From bedbc839aa33a58c410303f40ad359eae46d7b9a Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 19 Jun 2023 15:17:39 +0200 Subject: [PATCH 1/7] Update contact construction functions --- .../src/mechanisms/ant/mechanism.jl | 6 +- .../src/mechanisms/atlas/mechanism.jl | 6 +- .../src/mechanisms/block/mechanism.jl | 2 +- .../src/mechanisms/block2d/mechanism.jl | 2 +- .../src/mechanisms/halfcheetah/mechanism.jl | 4 +- .../src/mechanisms/hopper/mechanism.jl | 4 +- .../src/mechanisms/humanoid/mechanism.jl | 2 +- .../src/mechanisms/quadrotor/mechanism.jl | 4 +- .../src/mechanisms/quadruped/mechanism.jl | 6 +- .../src/mechanisms/raiberthopper/mechanism.jl | 4 +- .../src/mechanisms/snake/mechanism.jl | 2 +- .../src/mechanisms/sphere/mechanism.jl | 2 +- .../src/mechanisms/tippetop/mechanism.jl | 2 +- .../src/mechanisms/twister/mechanism.jl | 2 +- .../src/mechanisms/uuv/mechanism.jl | 2 +- .../src/mechanisms/walker/mechanism.jl | 4 +- .../collision_sphere_box.jl | 8 +- .../collision_sphere_capsule.jl | 15 +- .../collision_sphere_sphere.jl | 3 +- src/Dojo.jl | 3 +- src/contacts/constructor.jl | 129 ++++++++---------- src/contacts/impact.jl | 18 ++- src/contacts/linear.jl | 18 ++- src/contacts/nonlinear.jl | 18 ++- 24 files changed, 149 insertions(+), 117 deletions(-) diff --git a/DojoEnvironments/src/mechanisms/ant/mechanism.jl b/DojoEnvironments/src/mechanisms/ant/mechanism.jl index fc01c8792..59a324e64 100644 --- a/DojoEnvironments/src/mechanisms/ant/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/atlas/mechanism.jl b/DojoEnvironments/src/mechanisms/atlas/mechanism.jl index d826380d4..58cf786d9 100644 --- a/DojoEnvironments/src/mechanisms/atlas/mechanism.jl +++ b/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 diff --git a/DojoEnvironments/src/mechanisms/block/mechanism.jl b/DojoEnvironments/src/mechanisms/block/mechanism.jl index 6df7c931a..b002a8a86 100644 --- a/DojoEnvironments/src/mechanisms/block/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/block2d/mechanism.jl b/DojoEnvironments/src/mechanisms/block2d/mechanism.jl index 98be01c1e..04650e590 100644 --- a/DojoEnvironments/src/mechanisms/block2d/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl b/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl index 61f55b3ce..7bacd2de9 100644 --- a/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/hopper/mechanism.jl b/DojoEnvironments/src/mechanisms/hopper/mechanism.jl index 2d628497c..3865f36be 100644 --- a/DojoEnvironments/src/mechanisms/hopper/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl b/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl index 48a6d35ed..c5c3f348a 100644 --- a/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl b/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl index d637c88c4..b2c7085dc 100644 --- a/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl b/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl index d889b529f..8239e733a 100644 --- a/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl b/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl index cfadac2f9..1fa356bd8 100644 --- a/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/snake/mechanism.jl b/DojoEnvironments/src/mechanisms/snake/mechanism.jl index 3d8fcc9f5..87e40a4f9 100644 --- a/DojoEnvironments/src/mechanisms/snake/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/sphere/mechanism.jl b/DojoEnvironments/src/mechanisms/sphere/mechanism.jl index a5e07bd53..d75400961 100644 --- a/DojoEnvironments/src/mechanisms/sphere/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl b/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl index 1ddbc00c9..a9ef8e68d 100644 --- a/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/twister/mechanism.jl b/DojoEnvironments/src/mechanisms/twister/mechanism.jl index 7aa67f03f..b1a5de544 100644 --- a/DojoEnvironments/src/mechanisms/twister/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/uuv/mechanism.jl b/DojoEnvironments/src/mechanisms/uuv/mechanism.jl index 2270cd8d8..b1e589c1b 100644 --- a/DojoEnvironments/src/mechanisms/uuv/mechanism.jl +++ b/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; diff --git a/DojoEnvironments/src/mechanisms/walker/mechanism.jl b/DojoEnvironments/src/mechanisms/walker/mechanism.jl index 2bb3c53a5..80e9a20f0 100644 --- a/DojoEnvironments/src/mechanisms/walker/mechanism.jl +++ b/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; diff --git a/examples/simulation/object_collision_development/collision_sphere_box.jl b/examples/simulation/object_collision_development/collision_sphere_box.jl index d335bb147..2a89a6b77 100644 --- a/examples/simulation/object_collision_development/collision_sphere_box.jl +++ b/examples/simulation/object_collision_development/collision_sphere_box.jl @@ -27,11 +27,11 @@ contact_origins = [ normals = fill(Z_AXIS,8) friction_coefficients = fill(0.5,8) -box_contacts = contact_constraint(pbody, normals; - friction_coefficients, contact_origins, contact_type=:nonlinear) +box_contacts = ContactConstraint(NonlinearContact(pbody, normals, + friction_coefficients; contact_origins)) -sphere_contact = contact_constraint(cbody, Z_AXIS; - friction_coefficient=0.5, contact_radius=0.5) +sphere_contact = ContactConstraint(NonlinearContact(cbody, Z_AXIS, + 0.5; contact_radius=0.5)) collision = SphereBoxCollision{Float64,2,3,6}( szeros(3), 1.0, 1.0, 2 * 1.0, 0.5 diff --git a/examples/simulation/object_collision_development/collision_sphere_capsule.jl b/examples/simulation/object_collision_development/collision_sphere_capsule.jl index 7f7ecb0ec..9234687b5 100644 --- a/examples/simulation/object_collision_development/collision_sphere_capsule.jl +++ b/examples/simulation/object_collision_development/collision_sphere_capsule.jl @@ -12,19 +12,16 @@ joint2 = JointConstraint(Floating(pbody, cbody)) bodies = [pbody, cbody] joints = [joint1] -capsule_contact1 = contact_constraint(pbody, Z_AXIS, - friction_coefficient=1.0, +capsule_contact1 = ContactConstraint(NonlinearContact(pbody, Z_AXIS, 1.0; contact_origin=Y_AXIS, - contact_radius=0.5) + contact_radius=0.5)) -capsule_contact2 = contact_constraint(pbody, Z_AXIS, - friction_coefficient=1.0, +capsule_contact2 = ContactConstraint(NonlinearContact(pbody, Z_AXIS, 1.0; contact_origin=-Y_AXIS, - contact_radius=0.5) + contact_radius=0.5)) -sphere_contact = contact_constraint(cbody, Z_AXIS, - friction_coefficient=1.0, - contact_radius=0.5) +sphere_contact = ContactConstraint(NonlinearContact(cbody, Z_AXIS, 1.0; + contact_radius=0.5)) collision = SphereCapsuleCollision{Float64,2,3,6}( szeros(3), Y_AXIS, -Y_AXIS, 0.5, 0.5, diff --git a/examples/simulation/object_collision_development/collision_sphere_sphere.jl b/examples/simulation/object_collision_development/collision_sphere_sphere.jl index 8de8282e2..2539cfd28 100644 --- a/examples/simulation/object_collision_development/collision_sphere_sphere.jl +++ b/examples/simulation/object_collision_development/collision_sphere_sphere.jl @@ -11,8 +11,7 @@ joint = JointConstraint(Fixed(origin, pbody)) bodies = [pbody, cbody] joints = [joint] -sphere_contact = contact_constraint(cbody, Z_AXIS, - friction_coefficient=1.0, contact_radius=0.5) +sphere_contact = ContactConstraint(NonlinearContact(cbody, Z_AXIS, 1.0; contact_radius=0.5)) collision = SphereSphereCollision{Float64,2,3,6}( szeros(3), szeros(3), pbody.shape.r, cbody.shape.r) diff --git a/src/Dojo.jl b/src/Dojo.jl index 4561d6995..2bf8a1c92 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -193,8 +193,7 @@ export get_contact, get_sdf, contact_location, - damper_impulses, - contact_constraint + damper_impulses # Collision export diff --git a/src/contacts/constructor.jl b/src/contacts/constructor.jl index 23e2de752..e2a496fa2 100644 --- a/src/contacts/constructor.jl +++ b/src/contacts/constructor.jl @@ -26,22 +26,29 @@ mutable struct ContactConstraint{T,N,Nc,Cs,N½} <: Constraint{T,N} # variables impulses::Vector{SVector{N½,T}} impulses_dual::Vector{SVector{N½,T}} +end - function ContactConstraint(data; - name::Symbol=Symbol("contact_" * randstring(4))) +function ContactConstraint(data::Tuple; + name::Symbol=Symbol("contact_" * randstring(4))) - model, parent_id, child_id = data - T = typeof(model).parameters[1] + model, parent_id, child_id = data + T = typeof(model).parameters[1] - N = length(model) - N½ = Int64(N/2) + N = length(model) + N½ = Int64(N/2) - impulses = [neutral_vector(model) for i = 1:2] - impulses_dual = [neutral_vector(model) for i = 1:2] - new{T,N,1,typeof(model),N½}(getGlobalID(), name, model, parent_id, child_id, impulses, impulses_dual) - end + impulses = [neutral_vector(model) for i = 1:2] + impulses_dual = [neutral_vector(model) for i = 1:2] + ContactConstraint{T,N,1,typeof(model),N½}(getGlobalID(), name, model, parent_id, child_id, impulses, impulses_dual) +end + +function ContactConstraint(data::AbstractVector{<:Tuple}; + names::AbstractVector{Symbol}=[Symbol("contact_" * randstring(4)) for i = 1:length(data)]) + + [ContactConstraint(data[i]; name=names[i]) for i in eachindex(data)] end + # function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::ContactConstraint) # summary(io, constraint) # println(io, "") @@ -54,74 +61,56 @@ end # println(io, "impulses_dual: "*string(constraint.impulses_dual)) # end -""" - contact_constraint(bodies::Vector{Body}) - - generate ContactConstraints for each Body in list - - normals: surface normal for each contact point - friction_coefficients: value of coefficient of friction for each contact point (optional for ImpactContact) - contact_origins: the offset with respect to the center of Body for each contact point (optional) - contact_radius: radius for each contact (optional) - contact_type: :nonlinear, :linear, :impact -""" -function contact_constraint(bodies::Vector{Body{T}}, - normals::AbstractVector{<:AbstractVector}; - friction_coefficients::AbstractVector=ones(T,length(normals)), - contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], - contact_radii::AbstractVector=[0.0 for i=1:length(normals)], - names::Vector{Symbol}=[Symbol("contact_" * randstring(4)) for i = 1:length(normals)], - contact_type::Symbol=:nonlinear) where T - - n = length(normals) - @assert n == length(bodies) == length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) - contacts = Vector{ContactConstraint}() - for i = 1:n - contact = contact_constraint(bodies[i], normals[i], - friction_coefficient=friction_coefficients[i], - contact_origin=contact_origins[i], - contact_radius=contact_radii[i], - name=names[i], - contact_type=contact_type) - push!(contacts, contact) - end - contacts = [contacts...] # vector typing - return contacts -end - -function contact_constraint(body::Body{T}, - normals::AbstractVector{<:AbstractVector}; - friction_coefficients::AbstractVector=ones(T,length(normals)), - contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], - contact_radii::AbstractVector=[0.0 for i=1:length(normals)], - names::Vector{Symbol}=[Symbol("contact_" * randstring(4)) for i = 1:length(normals)], - contact_type::Symbol=:nonlinear) where T - n = length(normals) - @assert n == length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) - return contact_constraint(fill(body, n), normals; - friction_coefficients, contact_origins, contact_radii, names, contact_type) -end - -function contact_constraint(body::Body{T}, - normal::AbstractVector; - friction_coefficient=T(1), - contact_origin::AbstractVector=szeros(T, 3), - contact_radius=T(0), - name::Symbol=Symbol("contact_" * randstring(4)), - contact_type::Symbol=:nonlinear) where T +function ContactConstraint(contact_type::Symbol, body::Body{T}, normal::AbstractVector; + friction_coefficient=T(1), + contact_origin::AbstractVector=szeros(T, 3), + contact_radius=T(0), + name::Symbol=Symbol("contact_" * randstring(4))) where T if contact_type == :nonlinear - model = NonlinearContact(body, normal, friction_coefficient; + data = NonlinearContact(body, normal, friction_coefficient; contact_origin, contact_radius) elseif contact_type == :linear - model = LinearContact(body, normal, friction_coefficient; + data = LinearContact(body, normal, friction_coefficient; contact_origin, contact_radius) elseif contact_type == :impact - model = ImpactContact(body, normal; + data = ImpactContact(body, normal; contact_origin, contact_radius) else @warn "unknown contact_type" end - contacts = ContactConstraint((model, body.id, 0); name) - return contacts + + ContactConstraint(data; name) +end + + +function ContactConstraint(contact_type::Symbol, body::Body{T}, normals::AbstractVector{<:AbstractVector}; + friction_coefficients::AbstractVector=ones(T,length(normals)), + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)], + names::Vector{Symbol}=[Symbol("contact_" * randstring(4)) for i = 1:length(normals)]) where T + + @assert length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) == length(names) + [ContactConstraint(contact_type, body, normals[i]; friction_coefficient=friction_coefficients[i], contact_origin=contact_origins[i], contact_radius=contact_radii[i], name=names[i]) for i in eachindex(normals)] end + +""" + contact_constraint(bodies::Vector{Body}) + + generate ContactConstraints for each Body in list + + normals: surface normal for each contact point + friction_coefficients: value of coefficient of friction for each contact point (optional for ImpactContact) + contact_origins: the offset with respect to the center of Body for each contact point (optional) + contact_radius: radius for each contact (optional) + contact_type: :nonlinear, :linear, :impact +""" +function ContactConstraint(contact_type::Symbol, bodies::Vector{Body{T}}, normals::AbstractVector{<:AbstractVector}; + friction_coefficients::AbstractVector=ones(T,length(normals)), + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)], + names::Vector{Symbol}=[Symbol("contact_" * randstring(4)) for i = 1:length(normals)]) where T + + @assert length(bodies) == length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) == length(names) + [ContactConstraint(contact_type, bodies[i], normals[i]; friction_coefficient=friction_coefficients[i], contact_origin=contact_origins[i], contact_radius=contact_radii[i], name=names[i]) for i in eachindex(bodies)] +end \ No newline at end of file diff --git a/src/contacts/impact.jl b/src/contacts/impact.jl index e829b8930..eb80fbf8e 100644 --- a/src/contacts/impact.jl +++ b/src/contacts/impact.jl @@ -33,7 +33,23 @@ function ImpactContact(body::Body{T}, normal::AbstractVector; # collision collision = SphereHalfSpaceCollision(szeros(T, 0, 3), contact_normal, SVector{3}(contact_origin), contact_radius) - ImpactContact{Float64,2}(parameterization, collision) + ImpactContact{Float64,2}(parameterization, collision), body.id, 0 +end + +function ImpactContact(body::Body{T}, normals::AbstractVector{<:AbstractVector}; + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(normals) == length(contact_origins) == length(contact_radii) + [ImpactContact(body, normals[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(normals)] +end + +function ImpactContact(bodies::AbstractVector{Body{T}}, normals::AbstractVector{<:AbstractVector}; + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(bodies) == length(normals) == length(contact_origins) == length(contact_radii) + [ImpactContact(bodies[i], normals[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(bodies)] end function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs<:ImpactContact{T,N}} diff --git a/src/contacts/linear.jl b/src/contacts/linear.jl index 725dcca26..5b11e743d 100644 --- a/src/contacts/linear.jl +++ b/src/contacts/linear.jl @@ -42,7 +42,23 @@ function LinearContact(body::Body{T}, normal::AbstractVector, friction_coefficie # collision collision = SphereHalfSpaceCollision(contact_tangent, contact_normal, SVector{3}(contact_origin), contact_radius) - LinearContact{Float64,12}(friction_coefficient, parameterization, collision) + LinearContact{Float64,12}(friction_coefficient, parameterization, collision), body.id, 0 +end + +function LinearContact(body::Body{T}, normals::AbstractVector{<:AbstractVector}, friction_coefficients::AbstractVector=ones(T,length(normals)); + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) + [LinearContact(body, normals[i], friction_coefficients[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(normals)] +end + +function LinearContact(bodies::AbstractVector{Body{T}}, normals::AbstractVector{<:AbstractVector}, friction_coefficients::AbstractVector=ones(T,length(normals)); + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(bodies) == length(normals) == length(contact_origins) == length(contact_radii) + [LinearContact(bodies[i], normals[i], friction_coefficients[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(bodies)] end function constraint_jacobian(contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:LinearContact{T,N},N½} diff --git a/src/contacts/nonlinear.jl b/src/contacts/nonlinear.jl index 9388402b1..c7fffa158 100644 --- a/src/contacts/nonlinear.jl +++ b/src/contacts/nonlinear.jl @@ -43,7 +43,23 @@ function NonlinearContact(body::Body{T}, normal::AbstractVector, friction_coeffi # collision collision = SphereHalfSpaceCollision(contact_tangent, contact_normal, SVector{3}(contact_origin), contact_radius) - NonlinearContact{T,8}(friction_coefficient, parameterization, collision) + NonlinearContact{T,8}(friction_coefficient, parameterization, collision), body.id, 0 +end + +function NonlinearContact(body::Body{T}, normals::AbstractVector{<:AbstractVector}, friction_coefficients::AbstractVector=ones(T,length(normals)); + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) + [NonlinearContact(body, normals[i], friction_coefficients[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(normals)] +end + +function NonlinearContact(bodies::AbstractVector{Body{T}}, normals::AbstractVector{<:AbstractVector}, friction_coefficients::AbstractVector=ones(T,length(normals)); + contact_origins::AbstractVector=[szeros(T, 3) for i=1:length(normals)], + contact_radii::AbstractVector=[0.0 for i=1:length(normals)]) where T + + @assert length(bodies) == length(normals) == length(friction_coefficients) == length(contact_origins) == length(contact_radii) + [NonlinearContact(bodies[i], normals[i], friction_coefficients[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(bodies)] end function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} From c8ea9a8e649e80ec99a86120d5f25febcdd6d763 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 19 Jun 2023 15:19:33 +0200 Subject: [PATCH 2/7] Small bug fix for cayley map --- src/orientation/mapping.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/orientation/mapping.jl b/src/orientation/mapping.jl index c6dec5740..dbe914cb5 100644 --- a/src/orientation/mapping.jl +++ b/src/orientation/mapping.jl @@ -8,7 +8,7 @@ function quaternion_map_jacobian(ω::SVector{3}, timestep) end function cayley(ω) - Quaternion(1.0 / sqrt(1.0 + ω' * ω) * [1.0; ω]) + Quaternion(1.0 / sqrt(1.0 + ω' * ω) * [1.0; ω]...) end function cayley_jacobian(ω) From b467e6607ab2b43f27a5fe81cf4608844ba1e551 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 19 Jun 2023 15:20:06 +0200 Subject: [PATCH 3/7] Generalize integrator functions --- src/integrators/integrator.jl | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/integrators/integrator.jl b/src/integrators/integrator.jl index f3b90e4cb..ab0b46c2f 100644 --- a/src/integrators/integrator.jl +++ b/src/integrators/integrator.jl @@ -11,8 +11,8 @@ current_configuration_velocity(state::State) = (current_position(state), state.v initial_configuration_velocity(state::State) = (current_position(state), state.v15, current_orientation(state), state.ω15) # next -next_position(x2::SVector{3}, v25::SVector{3}, timestep::Real) = x2 + v25 * timestep -next_orientation(q2::Quaternion, ω25::SVector{3}, timestep::Real) = q2 * quaternion_map(ω25, timestep) * timestep / 2 +next_position(x2::AbstractVector, v25::AbstractVector, timestep::Real) = x2 + v25 * timestep +next_orientation(q2::Quaternion, ω25::AbstractVector, timestep::Real) = q2 * quaternion_map(ω25, timestep) * timestep / 2 next_position(state::State, timestep) = next_position(state.x2, state.vsol[2], timestep) next_orientation(state::State, timestep) = next_orientation(state.q2, state.ωsol[2], timestep) next_configuration(state::State, timestep) = (next_position(state, timestep), next_orientation(state, timestep)) @@ -32,13 +32,13 @@ function ∂angular_velocity∂q2(q1::Quaternion, q2::Quaternion, timestep) end # Jacobians -function integrator_jacobian_velocity(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ω25::SVector{3}, timestep::T) where T +function integrator_jacobian_velocity(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ω25::AbstractVector, timestep::T) where T V = [linear_integrator_jacobian_velocity(x2, v25, timestep) szeros(T,3,3)] Ω = [szeros(T,4,3) rotational_integrator_jacobian_velocity(q2, ω25, timestep)] return [V; Ω] # 7x6 end -function integrator_jacobian_configuration(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ω25::SVector{3}, timestep::T; +function integrator_jacobian_configuration(x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ω25::AbstractVector, timestep::T; attjac::Bool=true) where T Z = attjac ? szeros(T,3,3) : szeros(T,3,4) @@ -55,14 +55,14 @@ function linear_integrator_jacobian_velocity(x2::AbstractVector, v2::AbstractVec return timestep * SMatrix{3,3,T,9}(Diagonal(sones(T,3))) end -function rotational_integrator_jacobian_orientation(q2::Quaternion, ω25::SVector{3}, timestep::Real; +function rotational_integrator_jacobian_orientation(q2::Quaternion, ω25::AbstractVector, timestep::Real; attjac::Bool = true) M = Rmat(quaternion_map(ω25, timestep) * timestep / 2) attjac && (M *= LVᵀmat(q2)) return M end -function rotational_integrator_jacobian_velocity(q2::Quaternion, ω25::SVector{3}, timestep::Real) +function rotational_integrator_jacobian_velocity(q2::Quaternion, ω25::AbstractVector, timestep::Real) return Lmat(q2) * quaternion_map_jacobian(ω25, timestep) * timestep / 2 end From cd0e89876148e92bf57858923a519b8e73454edc Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 26 Jun 2023 13:11:08 +0200 Subject: [PATCH 4/7] Remove old body body collisions --- src/contacts/collisions/capsule_capsule.jl | 127 ---------- src/contacts/collisions/point_to_box.jl | 207 ---------------- src/contacts/collisions/point_to_box_v2.jl | 45 ---- src/contacts/collisions/point_to_segment.jl | 128 ---------- src/contacts/collisions/sphere_box.jl | 256 -------------------- src/contacts/collisions/sphere_capsule.jl | 190 --------------- src/contacts/collisions/sphere_sphere.jl | 195 --------------- src/contacts/collisions/string.jl | 252 ------------------- 8 files changed, 1400 deletions(-) delete mode 100644 src/contacts/collisions/capsule_capsule.jl delete mode 100644 src/contacts/collisions/point_to_box.jl delete mode 100644 src/contacts/collisions/point_to_box_v2.jl delete mode 100644 src/contacts/collisions/point_to_segment.jl delete mode 100644 src/contacts/collisions/sphere_box.jl delete mode 100644 src/contacts/collisions/sphere_capsule.jl delete mode 100644 src/contacts/collisions/sphere_sphere.jl delete mode 100644 src/contacts/collisions/string.jl diff --git a/src/contacts/collisions/capsule_capsule.jl b/src/contacts/collisions/capsule_capsule.jl deleted file mode 100644 index df395eb59..000000000 --- a/src/contacts/collisions/capsule_capsule.jl +++ /dev/null @@ -1,127 +0,0 @@ -""" - CapsuleCapsuleCollision - - collision between two capsules - - origin_parent: radius of contact for parent body - origin_child: radius of contact for child body - orientation_parent: radius of contact for parent body - orientation_child: radius of contact for child body - radius_parent: radius of contact for parent body - radius_child: radius of contact for child body - height_parent: radius of contact for parent body - height_child: radius of contact for child body -""" -mutable struct CapsuleCapsuleCollision{T,O,I,OI} <: Collision{T,O,I,OI} - origin_parent::SVector{I,T} - origin_child::SVector{I,T} - orientation_parent::Quaternion{T} - orientation_child::Quaternion{T} - radius_parent::T - radius_child::T - height_parent::T - height_child::T -end - -# function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, collision::CapsuleCapsuleCollision) -# summary(io, collision) -# println(io, "") -# println(io, "origin_parent: "*string(collision.origin_parent)) -# println(io, "origin_child: "*string(collision.origin_child)) -# println(io, "orientation_parent: "*string(collision.orientation_parent)) -# println(io, "orientation_child: "*string(collision.orientation_child)) -# println(io, "radius_parent: "*string(collision.radius_parent)) -# println(io, "radius_child: "*string(collision.radius_child)) -# println(io, "height_parent: "*string(collision.height_parent)) -# println(io, "height_child: "*string(collision.height_child)) -# end - -# distance -function distance(collision::CapsuleCapsuleCollision, xp, qp, xc, qc) - # contact points - pp = contact_point(:parent, collision, xp, qp, xc, qc) - pc = contact_point(:child, collision, xp, qp, xc, qc) - - # difference - d = pp - pc - - return norm(pp - pc, 2) -end - -function ∂distance∂x(gradient::Symbol, collision::CapsuleCapsuleCollision, xp, qp, xc, qc) - # contact points - pp = contact_point(:parent, collision, xp, qp, xc, qc) - pc = contact_point(:child, collision, xp, qp, xc, qc) - - # difference - d = pp - pc - - X = ∂norm∂x(d) * (∂contact_point∂x(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂x(:child, jacobian, collision, xp, qp, xc, qc)) - - return X -end - -function ∂distance∂q(gradient::Symbol, collision::CapsuleCapsuleCollision, xp, qp, xc, qc) - # contact points - pp = contact_point(:parent, collision, xp, qp, xc, qc) - pc = contact_point(:child, collision, xp, qp, xc, qc) - - # difference - d = pp - pc - - X = ∂norm∂x(d) * (∂contact_point∂q(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂q(:child, jacobian, collision, xp, qp, xc, qc)) - - return X -end - -# contact point in world frame -function contact_point(relative::Symbol, collision::CapsuleCapsuleCollision, xp, qp, xc, qc) - # call mehrotra - - if relative == :parent - return collision.ip.z[SA[1;2;3]] - elseif relative == :child - return collision.ip.z[SA[4;5;6]] - end -end - -function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::CapsuleCapsuleCollision, xp, qp, xc, qc) - # call mehrotra - - if relative == :parent - if jacobian == :parent - X = collision.ip.δz[] - elseif jacobian == :child - X = collision.ip.δz[] - end - elseif relative == :child - if jacobian == :parent - X = collision.ip.δz[] - elseif jacobian == :child - X = collision.ip.δz[] - end - end - - return X -end - -function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::CapsuleCapsuleCollision, xp, qp, xc, qc) -# call mehrotra - - if relative == :parent - if jacobian == :parent - X = collision.ip.δz[] - elseif jacobian == :child - X = collision.ip.δz[] - end - elseif relative == :child - if jacobian == :parent - X = collision.ip.δz[] - elseif jacobian == :child - X = collision.ip.δz[] - end - end - - return X -end - diff --git a/src/contacts/collisions/point_to_box.jl b/src/contacts/collisions/point_to_box.jl deleted file mode 100644 index 637ee1784..000000000 --- a/src/contacts/collisions/point_to_box.jl +++ /dev/null @@ -1,207 +0,0 @@ -function contact_point_box(ps, xc, qc, kx, ky, kz) - # corner kinematics - cx = contact_point_origin(szeros(3), qc, kx) - cy = contact_point_origin(szeros(3), qc, ky) - cz = contact_point_origin(szeros(3), qc, kz) - - v1 = 2.0 * cx - v2 = 2.0 * cy - v3 = 2.0 * cz - - # origin is opposite corner - origin = xc - cx - cy - cz - - @show tx = dot(ps - origin, v1) / dot(v1, v1) - @show ty = dot(ps - origin, v2) / dot(v2, v2) - @show tz = dot(ps - origin, v3) / dot(v3, v3) - - coc = origin - - if tx >= 1.0 - coc += v1 - elseif tx < 0.0 - nothing - else - coc += tx * v1 - end - - if ty >= 1.0 - coc += v2 - elseif ty < 0.0 - nothing - else - coc += ty * v2 - end - - if tz >= 1.0 - coc += v3 - elseif tz < 0.0 - nothing - else - coc += tz * v3 - end - - return coc -end - -function ∂contact_point_box∂p(ps, xc, qc, kx, ky, kz) - cx = contact_point_origin(szeros(3), qc, kx) - cy = contact_point_origin(szeros(3), qc, ky) - cz = contact_point_origin(szeros(3), qc, kz) - - v1 = 2.0 * cx - v2 = 2.0 * cy - v3 = 2.0 * cz - - origin = xc - cx - cy - cz - - tx = dot(ps - origin, v1) / dot(v1, v1) - ty = dot(ps - origin, v2) / dot(v2, v2) - tz = dot(ps - origin, v3) / dot(v3, v3) - - P = szeros(3, 3) - - if tx >= 1.0 - # coc += v1 - # X += - elseif tx < 0.0 - # nothing - # X += - else - # coc += tx * v1 - P += v1 * 1.0 / dot(v1, v1) * v1' - end - - if ty >= 1.0 - # coc += v2 - elseif ty < 0.0 - # nothing - else - # coc += ty * v2 - P += v2 * 1.0 / dot(v2, v2) * v2' - end - - if tz >= 1.0 - # coc += v3 - elseif tz < 0.0 - # nothing - else - # coc += tz * v3 - P += v3 * 1.0 / dot(v3, v3) * v3' - end - - return P - # FiniteDiff.finite_difference_jacobian(p -> contact_point_box(p, xc, qc, kx, ky, kz), ps) -end - -function ∂contact_point_box∂x(ps, xc, qc, kx, ky, kz) - cx = contact_point_origin(szeros(3), qc, kx) - cy = contact_point_origin(szeros(3), qc, ky) - cz = contact_point_origin(szeros(3), qc, kz) - - v1 = 2.0 * cx - v2 = 2.0 * cy - v3 = 2.0 * cz - - origin = xc - cx - cy - cz - - tx = dot(ps - origin, v1) / dot(v1, v1) - ty = dot(ps - origin, v2) / dot(v2, v2) - tz = dot(ps - origin, v3) / dot(v3, v3) - - X = 1.0 * sI(3) - - if tx >= 1.0 - # coc += v1 - elseif tx < 0.0 - # nothing - else - # coc += tx * v1 - X += v1 * 1.0 / dot(v1, v1) * v1' * -1.0 - end - - if ty >= 1.0 - # coc += v2 - elseif ty < 0.0 - # nothing - else - # coc += ty * v2 - X += v2 * 1.0 / dot(v2, v2) * v2' * -1.0 - end - - if tz > 1.0 - # coc += v3 - elseif tz < 0.0 - # nothing - else - # coc += tz * v3 - X += v3 * 1.0 / dot(v3, v3) * v3' * -1.0 - end - - return X - # FiniteDiff.finite_difference_jacobian(x -> contact_point_box(ps, x, qc, kx, ky, kz), xc) -end - -function ∂contact_point_box∂q(ps, xc, qc, kx, ky, kz) - cx = contact_point_origin(szeros(3), qc, kx) - cy = contact_point_origin(szeros(3), qc, ky) - cz = contact_point_origin(szeros(3), qc, kz) - - v1 = 2.0 * cx - v2 = 2.0 * cy - v3 = 2.0 * cz - - ∂v1∂q = 2.0 * ∂contact_point_origin∂q(szeros(3), qc, kx) - ∂v2∂q = 2.0 * ∂contact_point_origin∂q(szeros(3), qc, ky) - ∂v3∂q = 2.0 * ∂contact_point_origin∂q(szeros(3), qc, kz) - - origin = xc - cx - cy - cz - - tx = dot(ps - origin, v1) / dot(v1, v1) - ty = dot(ps - origin, v2) / dot(v2, v2) - tz = dot(ps - origin, v3) / dot(v3, v3) - - Q = -0.5 * (∂v1∂q + ∂v2∂q + ∂v3∂q) - - if tx >= 1.0 - # coc += v1 - Q += ∂v1∂q - elseif tx < 0.0 - # nothing - else - # coc += tx * v1 - Q += tx * ∂v1∂q - Q += v1 * (ps - xc)' ./ dot(v1, v1) * ∂v1∂q - Q += v1 * -1.0 * dot(ps - xc, v1) ./ dot(v1, v1)^2 * 2.0 * v1' * ∂v1∂q - Q += v1 * 1.0 / dot(v1, v1) * v1' * -0.5 * ∂v1∂q - end - - if ty >= 1.0 - # coc += v2 - Q += ∂v2∂q - elseif ty < 0.0 - # nothing - else - # coc += ty * v2 - Q += tx * ∂v2∂q - Q += v2 * (ps - xc)' ./ dot(v2, v2) * ∂v2∂q - Q += v2 * -1.0 * dot(ps - xc, v2) ./ dot(v2, v2)^2 * 2.0 * v2' * ∂v2∂q - Q += v2 * 1.0 / dot(v2, v2) * v2' * -0.5 * ∂v2∂q - end - - if tz >= 1.0 - # coc += v3 - Q += ∂v3∂q - elseif tz < 0.0 - # nothing - else - # coc += tz * v3 - Q += tx * ∂v3∂q - Q += v3 * (ps - xc)' ./ dot(v3, v3) * ∂v3∂q - Q += v3 * -1.0 * dot(ps - xc, v3) ./ dot(v3, v3)^2 * 2.0 * v3' * ∂v3∂q - Q += v3 * 1.0 / dot(v3, v3) * v3' * -0.5 * ∂v3∂q - end - - return Q - # FiniteDiff.finite_difference_jacobian(q -> contact_point_box(ps, xc, Quaternion(q...), kx, ky, kz), vector(qc)) -end diff --git a/src/contacts/collisions/point_to_box_v2.jl b/src/contacts/collisions/point_to_box_v2.jl deleted file mode 100644 index d8bd5bd09..000000000 --- a/src/contacts/collisions/point_to_box_v2.jl +++ /dev/null @@ -1,45 +0,0 @@ -function klamp(t, a, b) - if t <= a - return a - elseif b <= t - return b - else - da = abs(t - a) - db = abs(t - b) - - if da < db - return a - else - return b - end - # return t - end -end - -function contact_point_box(ps, xc, qc, kx, ky, kz) - # rotate point into box frame - ps_box_frame = vector_rotate(ps - xc, inv(qc)) - - # find box point in box frame - px = clamp(ps_box_frame[1], -0.5 * kx, 0.5 * kx) - py = clamp(ps_box_frame[2], -0.5 * ky, 0.5 * ky) - pz = clamp(ps_box_frame[3], -0.5 * kz, 0.5 * kz) - - # box point in world frame - return vector_rotate([px; py; pz], qc) + xc -end - -function ∂contact_point_box∂p(ps, xc, qc, kx, ky, kz) - - FiniteDiff.finite_difference_jacobian(p -> contact_point_box(p, xc, qc, kx, ky, kz), ps) -end - -function ∂contact_point_box∂x(ps, xc, qc, kx, ky, kz) - - FiniteDiff.finite_difference_jacobian(x -> contact_point_box(ps, x, qc, kx, ky, kz), xc) -end - -function ∂contact_point_box∂q(ps, xc, qc, kx, ky, kz) - - FiniteDiff.finite_difference_jacobian(q -> contact_point_box(ps, xc, Quaternion(q...), kx, ky, kz), vector(qc)) -end diff --git a/src/contacts/collisions/point_to_segment.jl b/src/contacts/collisions/point_to_segment.jl deleted file mode 100644 index a413b936a..000000000 --- a/src/contacts/collisions/point_to_segment.jl +++ /dev/null @@ -1,128 +0,0 @@ -# capsule contact point origin sphere-capsule -function contact_point_segment(ps, xc, qc, ka, kb) - # capsule origin a - ca = contact_point_origin(xc, qc, ka) - - # capsule origin b - cb = contact_point_origin(xc, qc, kb) - - # point differences - dab = cb - ca - das = ps - ca - - t = dot(das, dab) / dot(dab, dab) - # t_clamp = min(max(t, 0.0), 1.0) - - # closest point - if t <= 0.0 - p = ca - elseif t >= 1.0 - p = cb - else - p = ca + t * dab - end - - return p -end - -# capsule contact point origin sphere-capsule -function ∂contact_point_segment∂p(ps, xc, qc, ka, kb) - # capsule origin a - ca = contact_point_origin(xc, qc, ka) - - # capsule origin b - cb = contact_point_origin(xc, qc, kb) - - # point differences - dab = cb - ca - das = ps - ca - - t = dot(das, dab) ./ dot(dab, dab) - # t_clamp = min(max(t, 0.0), 1.0) - - # closest point - if t <= 0.0 - # p = ca - return szeros(3, 3) - elseif t >= 1.0 - # p = cb - return szeros(3, 3) - else - # p = ca + t * dab - return dab * dab' ./ dot(dab, dab) - end - - return p -end - -# capsule contact point origin sphere-capsule -function ∂contact_point_segment∂x(ps, xc, qc, ka, kb) - # capsule origin a - ca = contact_point_origin(xc, qc, ka) - - # capsule origin b - cb = contact_point_origin(xc, qc, kb) - - # point differences - dab = cb - ca - das = ps - ca - - t = dot(das, dab) ./ dot(dab, dab) - # t_clamp = min(max(t, 0.0), 1.0) - - # closest point - if t <= 0.0 - # p = ca - return ∂contact_point_origin∂x(xc, qc, ka) - elseif t >= 1.0 - # p = cb - return ∂contact_point_origin∂x(xc, qc, kb) - else - # p = ca + t * dab - X = ∂contact_point_origin∂x(xc, qc, ka) - X += t * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka)) - - t1 = 1.0 / (dab' * dab) * dab' * (-∂contact_point_origin∂x(xc, qc, ka)) - t2 = 1.0 / (dab' * dab) * das' * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka)) - t3 = -2.0 * das' * dab / (dab' * dab)^2 * dab' * (∂contact_point_origin∂x(xc, qc, kb) - ∂contact_point_origin∂x(xc, qc, ka)) - - X += dab * (t1 + t2 + t3) - return X - end -end - -# capsule contact point origin sphere-capsule -function ∂contact_point_segment∂q(ps, xc, qc, ka, kb) - # capsule origin a - ca = contact_point_origin(xc, qc, ka) - - # capsule origin b - cb = contact_point_origin(xc, qc, kb) - - # point differences - dab = cb - ca - das = ps - ca - - t = dot(das, dab) ./ dot(dab, dab) - # t_clamp = min(max(t, 0.0), 1.0) - - # closest point - if t <= 0.0 - # p = ca - return ∂contact_point_origin∂q(xc, qc, ka) - elseif t >= 1.0 - # p = cb - return ∂contact_point_origin∂q(xc, qc, kb) - else - # p = ca + t * dab - Q = ∂contact_point_origin∂q(xc, qc, ka) - Q += t * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka)) - - t1 = 1.0 / (dab' * dab) * dab' * (-∂contact_point_origin∂q(xc, qc, ka)) - t2 = 1.0 / (dab' * dab) * das' * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka)) - t3 = -2.0 * das' * dab ./ (dab' * dab)^2 * dab' * (∂contact_point_origin∂q(xc, qc, kb) - ∂contact_point_origin∂q(xc, qc, ka)) - Q += dab * (t1 + t2 + t3) - - return Q - end -end \ No newline at end of file diff --git a/src/contacts/collisions/sphere_box.jl b/src/contacts/collisions/sphere_box.jl deleted file mode 100644 index f97bf6027..000000000 --- a/src/contacts/collisions/sphere_box.jl +++ /dev/null @@ -1,256 +0,0 @@ -""" - SphereBoxCollision - - collision between sphere and box - - origin_sphere: position of sphere contact relative to body center of mass - origin_box_a: position of box corner contact a relative to body center of mass - origin_box_b: position of box corner contact b relative to body center of mass - radius_sphere: radius of sphere contact -""" -mutable struct SphereBoxCollision{T,O,I,OI} <: Collision{T,O,I,OI} - origin_sphere::SVector{I,T} - width::T - depth::T - height::T - radius_sphere::T -end - -# distance -function distance(collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # distance between contact origins - d = norm(cop - coc, 2) - - # minimum distance between spheres - return d - collision.radius_sphere -end - -function ∂distance∂x(gradient::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * (1.0 * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere)) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_box∂x(cop, xc, qc, collision.width, collision.depth, collision.height) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, x, qp, xc, qc), xp) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, xp, qp, x, qc), xc) - # end - - # return FD - - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -function ∂distance∂q(gradient::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * (1.0 * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere)) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_box∂q(cop, xc, qc, collision.width, collision.depth, collision.height) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # return FD - - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -# contact point in world frame -function contact_point(relative::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - # contact point - if relative == :parent - return cop - collision.radius_sphere * dir - elseif relative == :child - return coc - end -end - -function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_sphere * dir - if jacobian == :parent - ∂c∂x = ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - X = ∂c∂x - X -= collision.radius_sphere * ∂normalize∂x(d) * (∂c∂x - ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂c∂x) - elseif jacobian == :child - X = -1.0 * collision.radius_sphere * ∂normalize∂x(d) * -1.0 * ∂contact_point_box∂x(cop, xc, qc, collision.width, collision.depth, collision.height) - end - elseif relative == :child - # coc - if jacobian == :parent - X = ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - elseif jacobian == :child - X = ∂contact_point_box∂x(cop, xc, qc, collision.width, collision.depth, collision.height) - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, x, qp, xc, qc), xp) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, xp, qp, x, qc), xc) - # end - - # return FD - - # @assert norm(X - FD, Inf) < 1.0e-5 - - return X -end - -function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_box(cop, xc, qc, collision.width, collision.depth, collision.height) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_sphere * dir - if jacobian == :parent - ∂c∂q = ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - Q = ∂c∂q - Q -= collision.radius_sphere * ∂normalize∂x(d) * (∂c∂q - ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂c∂q) - elseif jacobian == :child - Q = -1.0 * collision.radius_sphere * ∂normalize∂x(d) * -1.0 * ∂contact_point_box∂q(cop, xc, qc, collision.width, collision.depth, collision.height) - end - elseif relative == :child - # coc - if jacobian == :parent - Q = ∂contact_point_box∂p(cop, xc, qc, collision.width, collision.depth, collision.height) * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - elseif jacobian == :child - Q = ∂contact_point_box∂q(cop, xc, qc, collision.width, collision.depth, collision.height) - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # return FD - - # @assert norm(Q - FD, Inf) < 1.0e-5 - - return Q -end - - -function contact_normal(collision::SphereBoxCollision, xp, qp, xc, qc) - # contact points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - # distance - dis = distance(collision, xp, qp, xc, qc) - - # normalized direction - if dis >= 0.0 - return normalize(dir)' - else - return -1.0 * normalize(dir)' - end -end - -function ∂contact_normal_transpose∂x(jacobian::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - # Jacobians - X = ∂normalize∂x(dir) * (∂contact_point∂x(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂x(:child, jacobian, collision, xp, qp, xc, qc)) - - # distance - dis = distance(collision, xp, qp, xc, qc) - - # normalized direction - if dis >= 0.0 - return X - else - return -1.0 * X - end -end - -function ∂contact_normal_transpose∂q(jacobian::Symbol, collision::SphereBoxCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - Q = ∂normalize∂x(dir) * (∂contact_point∂q(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂q(:child, jacobian, collision, xp, qp, xc, qc)) - - # Jacobians - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xp, Quaternion(q...), xc, qc)', vector(qp)) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xp, qp, xc, Quaternion(q...))', vector(qc)) - # end - - # distance - dis = distance(collision, xp, qp, xc, qc) - - # @assert norm((dis >= 0.0 ? 1.0 : -1.0) * Q - FD, Inf) < 1.0e-2 - - # normalized direction - if dis >= 0.0 - return Q - else - return -1.0 * Q - end -end \ No newline at end of file diff --git a/src/contacts/collisions/sphere_capsule.jl b/src/contacts/collisions/sphere_capsule.jl deleted file mode 100644 index 899aaacc8..000000000 --- a/src/contacts/collisions/sphere_capsule.jl +++ /dev/null @@ -1,190 +0,0 @@ -""" - SphereCapsuleCollision - - collision between sphere and capsule - - origin_sphere: position of sphere contact relative to body center of mass - origin_capsule_a: position of capsule contact a relative to body center of mass - origin_capsule_b: position of capsule contact b relative to body center of mass - radius_sphere: radius of sphere contact - radius_capsule: radius of capsule contact -""" -mutable struct SphereCapsuleCollision{T,O,I,OI} <: Collision{T,O,I,OI} - origin_sphere::SVector{I,T} - origin_capsule_a::SVector{I,T} - origin_capsule_b::SVector{I,T} - radius_sphere::T - radius_capsule::T -end - -# distance -function distance(collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # distance between contact origins - d = norm(cop - coc, 2) - - # minimum distance between spheres - return d - (collision.radius_sphere + collision.radius_capsule) -end - -function ∂distance∂x(gradient::Symbol, collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * (1.0 * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere)) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_segment∂x(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, x, qp, xc, qc), xp) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, xp, qp, x, qc), xc) - # end - - # return FD - - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -function ∂distance∂q(gradient::Symbol, collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * (1.0 * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere)) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_segment∂q(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # return FD - - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -# contact point in world frame -function contact_point(relative::Symbol, collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - # contact point - if relative == :parent - return cop - collision.radius_sphere * dir - elseif relative == :child - return coc + collision.radius_capsule * dir - end -end - -function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_sphere * dir - if jacobian == :parent - ∂c∂x = ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - X = ∂c∂x - X -= collision.radius_sphere * ∂normalize∂x(d) * (∂c∂x - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂c∂x) - elseif jacobian == :child - X = -1.0 * collision.radius_sphere * ∂normalize∂x(d) * -1.0 * ∂contact_point_segment∂x(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - end - elseif relative == :child - # coc + collision.radius_capsule * dir - if jacobian == :parent - X = ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - X += 1.0 * collision.radius_capsule * ∂normalize∂x(d) * (∂contact_point_origin∂x(xp, qp, collision.origin_sphere) - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂x(xp, qp, collision.origin_sphere)) - elseif jacobian == :child - X = ∂contact_point_segment∂x(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - X += collision.radius_capsule * ∂normalize∂x(d) * (-∂contact_point_segment∂x(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b)) - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, x, qp, xc, qc), xp) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, xp, qp, x, qc), xc) - # end - - # return FD - - # @assert norm(X - FD, Inf) < 1.0e-5 - - return X -end - -function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::SphereCapsuleCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_sphere) - coc = contact_point_segment(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_sphere * dir - if jacobian == :parent - ∂c∂q = ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - Q = ∂c∂q - Q -= collision.radius_sphere * ∂normalize∂x(d) * (∂c∂q - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂c∂q) - elseif jacobian == :child - Q = -1.0 * collision.radius_sphere * ∂normalize∂x(d) * -1.0 * ∂contact_point_segment∂q(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - end - elseif relative == :child - # coc + collision.radius_capsule * dir - if jacobian == :parent - Q = ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - Q += 1.0 * collision.radius_capsule * ∂normalize∂x(d) * (∂contact_point_origin∂q(xp, qp, collision.origin_sphere) - ∂contact_point_segment∂p(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) * ∂contact_point_origin∂q(xp, qp, collision.origin_sphere)) - elseif jacobian == :child - Q = ∂contact_point_segment∂q(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b) - Q += collision.radius_capsule * ∂normalize∂x(d) * (-∂contact_point_segment∂q(cop, xc, qc, collision.origin_capsule_a, collision.origin_capsule_b)) - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # return FD - - # @assert norm(Q - FD, Inf) < 1.0e-5 - - return Q -end - diff --git a/src/contacts/collisions/sphere_sphere.jl b/src/contacts/collisions/sphere_sphere.jl deleted file mode 100644 index eac3a1d96..000000000 --- a/src/contacts/collisions/sphere_sphere.jl +++ /dev/null @@ -1,195 +0,0 @@ -""" - SphereSphereCollision - - collision between two spheres - - origin_parent: position of contact on parent body relative to center of mass - origin_child: position of contact on parent body relative to center of mass - radius_parent: radius of contact for parent body - radius_child: radius of contact for child body -""" -mutable struct SphereSphereCollision{T,O,I,OI} <: Collision{T,O,I,OI} - origin_parent::SVector{I,T} - origin_child::SVector{I,T} - radius_parent::T - radius_child::T -end - -# function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, collision::SphereSphereCollision) -# summary(io, collision) -# println(io, "") -# println(io, "origin_parent: "*string(collision.origin_parent)) -# println(io, "origin_child: "*string(collision.origin_child)) -# println(io, "radius_parent: "*string(collision.radius_parent)) -# println(io, "radius_child: "*string(collision.radius_child)) -# end - -# distance -function distance(collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - d = norm(cop - coc, 2) - - # minimum distance between spheres - return d - (collision.radius_parent + collision.radius_child) -end - -function ∂distance∂x(gradient::Symbol, collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * 1.0 * ∂contact_point_origin∂x(xp, qp, collision.origin_parent) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_origin∂x(xc, qc, collision.origin_child) - end - - if gradient == :parent - FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, x, qp, xc, qc), xp) - elseif gradient == :child - FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, xp, qp, x, qc), xc) - end - - return FD - - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -function ∂distance∂q(gradient::Symbol, collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = ∂norm∂d * 1.0 * ∂contact_point_origin∂q(xp, qp, collision.origin_parent) - elseif gradient == :child - D = ∂norm∂d * -1.0 * ∂contact_point_origin∂q(xc, qc, collision.origin_child) - end - - if gradient == :parent - FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, Quaternion(q...), xc, qc), vector(qp)) - elseif gradient == :child - FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - end - - return FD - # @assert norm(D - FD, Inf) < 1.0e-5 - - return D -end - -# contact point in world frame -function contact_point(relative::Symbol, collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - # contact point - if relative == :parent - return cop - collision.radius_parent * dir - elseif relative == :child - return coc + collision.radius_child * dir - end -end - -function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_parent * dir - if jacobian == :parent - ∂c∂x = ∂contact_point_origin∂x(xp, qp, collision.origin_parent) - X = ∂c∂x - X -= collision.radius_parent * ∂normalize∂x(d) * ∂c∂x - elseif jacobian == :child - X = -1.0 * collision.radius_parent * ∂normalize∂x(d) * -1.0 * ∂contact_point_origin∂x(xc, qc, collision.origin_child) - end - elseif relative == :child - # coc + collision.radius_child * dir - if jacobian == :parent - X = 1.0 * collision.radius_child * ∂normalize∂x(d) * ∂contact_point_origin∂x(xp, qp, collision.origin_parent) - elseif jacobian == :child - ∂c∂x = ∂contact_point_origin∂x(xc, qc, collision.origin_child) - X = ∂c∂x - X += collision.radius_child * ∂normalize∂x(d) * -1.0 * ∂c∂x - end - end - - if jacobian == :parent - FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, x, qp, xc, qc), xp) - elseif jacobian == :child - FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, xp, qp, x, qc), xc) - end - - return FD - # @assert norm(X - FD, Inf) < 1.0e-5 - - return X -end - -function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::SphereSphereCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - d = cop - coc - dir = normalize(d) - - if relative == :parent - # cop - collision.radius_parent * dir - if jacobian == :parent - ∂c∂q = ∂contact_point_origin∂q(xp, qp, collision.origin_parent) - Q = ∂c∂q - Q -= collision.radius_parent * ∂normalize∂x(d) * ∂c∂q - elseif jacobian == :child - Q = -1.0 * collision.radius_parent * ∂normalize∂x(d) * -1.0 * ∂contact_point_origin∂q(xc, qc, collision.origin_child) - end - elseif relative == :child - # coc + collision.radius_child * dir - if jacobian == :parent - Q = 1.0 * collision.radius_child * ∂normalize∂x(d) * ∂contact_point_origin∂q(xp, qp, collision.origin_parent) - elseif jacobian == :child - ∂c∂q = ∂contact_point_origin∂q(xc, qc, collision.origin_child) - Q = ∂c∂q - Q += collision.radius_child * ∂normalize∂x(d) * -1.0 * ∂c∂q - end - end - - if jacobian == :parent - FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, Quaternion(q...), xc, qc), vector(qp)) - elseif jacobian == :child - FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - end - - return FD - - # @assert norm(Q - FD, Inf) < 1.0e-5 - - return Q -end - diff --git a/src/contacts/collisions/string.jl b/src/contacts/collisions/string.jl deleted file mode 100644 index f30880311..000000000 --- a/src/contacts/collisions/string.jl +++ /dev/null @@ -1,252 +0,0 @@ -#=""" - StringCollision - - string collision between contact points - - origin_parent: position of contact on parent body relative to center of mass - origin_child: position of contact on parent body relative to center of mass - length: maximum distance between contact point -"""=# -mutable struct StringCollision{T,O,I,OI} <: Collision{T,O,I,OI} - origin_parent::SVector{I,T} - origin_child::SVector{I,T} - length::T -end - -# function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, collision::StringCollision) -# summary(io, collision) -# println(io, "") -# println(io, "origin_parent: "*string(collision.origin_parent)) -# println(io, "origin_child: "*string(collision.origin_child)) -# println(io, "length: "*string(collision.length)) -# end - -# distance -function distance(collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - d = norm(cop - coc, 2) - - # minimum distance between spheres - return collision.length - d -end - -function ∂distance∂x(gradient::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - # d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = 1.0 * ∂norm∂d * 1.0 * ∂contact_point_origin∂x(xp, qp, collision.origin_parent) - elseif gradient == :child - D = 1.0 * ∂norm∂d * -1.0 * ∂contact_point_origin∂x(xc, qc, collision.origin_child) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, x, qp, xc, qc), xp) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, xp, qp, x, qc), xc) - # end - - # @assert norm(-D - FD, Inf) < 1.0e-5 - - return -D -end - -function ∂distance∂q(gradient::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # distance between contact origins - # d = norm(cop - coc, 2) - ∂norm∂d = ∂norm∂x(cop - coc) - - if gradient == :parent - D = 1.0 * ∂norm∂d * 1.0 * ∂contact_point_origin∂q(xp, qp, collision.origin_parent) - elseif gradient == :child - D = 1.0 * ∂norm∂d * -1.0 * ∂contact_point_origin∂q(xc, qc, collision.origin_child) - end - - # if gradient == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif gradient == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # @assert norm(-D - FD, Inf) < 1.0e-5 - - return -D -end - -# contact point in world frame -function contact_point(relative::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - # d = cop - coc - # dir = normalize(d) - - # contact point - if relative == :parent - return cop - elseif relative == :child - return coc - end -end - -function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - # d = cop - coc - # dir = normalize(d) - - if relative == :parent - # cop - if jacobian == :parent - ∂c∂x = ∂contact_point_origin∂x(xp, qp, collision.origin_parent) - X = ∂c∂x - elseif jacobian == :child - X = szeros(eltype(xc), 3, 3) - end - elseif relative == :child - # coc - if jacobian == :parent - X = szeros(eltype(xp), 3, 3) - elseif jacobian == :child - ∂c∂x = ∂contact_point_origin∂x(xc, qc, collision.origin_child) - X = ∂c∂x - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, x, qp, xc, qc), xp) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, xp, qp, x, qc), xc) - # end - - # @assert norm(X - FD, Inf) < 1.0e-5 - - return X -end - -function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point_origin(xp, qp, collision.origin_parent) - coc = contact_point_origin(xc, qc, collision.origin_child) - - # direction of minimum distance (child to parent) - # d = cop - coc - # dir = normalize(d) - - if relative == :parent - # cop - if jacobian == :parent - ∂c∂q = ∂contact_point_origin∂q(xp, qp, collision.origin_parent) - Q = ∂c∂q - elseif jacobian == :child - Q = szeros(eltype(xc), 3, 4) - end - elseif relative == :child - # coc - if jacobian == :parent - Q = szeros(eltype(xp), 3, 4) - elseif jacobian == :child - ∂c∂q = ∂contact_point_origin∂q(xc, qc, collision.origin_child) - Q = ∂c∂q - end - end - - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, Quaternion(q...), xc, qc), vector(qp)) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, qp, xc, Quaternion(q...)), vector(qc)) - # end - - # @assert norm(Q - FD, Inf) < 1.0e-5 - - return Q -end - -function contact_normal(collision::StringCollision, xp, qp, xc, qc) - # contact points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - # distance - dis = distance(collision, xp, qp, xc, qc) - - # normalized direction - # if dis >= 0.0 - return -normalize(dir)' - # else - # return normalize(dir)' - # end -end - -function ∂contact_normal_transpose∂x(jacobian::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - # Jacobians - X = ∂normalize∂x(dir) * (∂contact_point∂x(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂x(:child, jacobian, collision, xp, qp, xc, qc)) - - # distance - # dis = distance(collision, xp, qp, xc, qc) - - # normalized direction - # if dis >= 0.0 - return -X - # else - # return 1.0 * X - # end -end - -function ∂contact_normal_transpose∂q(jacobian::Symbol, collision::StringCollision, xp, qp, xc, qc) - # contact origin points - cop = contact_point(:parent, collision, xp, qp, xc, qc) - coc = contact_point(:child, collision, xp, qp, xc, qc) - - # unnormalized direction - dir = cop - coc - - Q = ∂normalize∂x(dir) * (∂contact_point∂q(:parent, jacobian, collision, xp, qp, xc, qc) - ∂contact_point∂q(:child, jacobian, collision, xp, qp, xc, qc)) - - # Jacobians - # if jacobian == :parent - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xp, Quaternion(q...), xc, qc)', vector(qp)) - # elseif jacobian == :child - # FD = FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xp, qp, xc, Quaternion(q...))', vector(qc)) - # end - - # distance - dis = distance(collision, xp, qp, xc, qc) - - # @assert norm((dis >= 0.0 ? 1.0 : -1.0) * Q - FD, Inf) < 1.0e-2 - - # normalized direction - # if dis >= 0.0 - return -Q - # else - # return Q - # end -end From 6da24ea726e6e9caea881dd671ad73b7cab951a2 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 26 Jun 2023 13:57:14 +0200 Subject: [PATCH 5/7] Restructure joints --- src/joints/constraints.jl | 324 -------------------------------------- src/joints/constructor.jl | 99 ++++++++++++ src/joints/impulses.jl | 114 ++++++++++++++ src/joints/input.jl | 42 +++++ src/joints/minimal.jl | 17 ++ 5 files changed, 272 insertions(+), 324 deletions(-) create mode 100644 src/joints/constructor.jl create mode 100644 src/joints/input.jl diff --git a/src/joints/constraints.jl b/src/joints/constraints.jl index 9d0c29482..166038f30 100644 --- a/src/joints/constraints.jl +++ b/src/joints/constraints.jl @@ -1,116 +1,3 @@ -""" - JointConstraint{T} <: Constraint{T} - - constraint restricting translational and rotational degrees of freedom between two Body objects. - - id: a unique identifying number - name: a unique identifying name - translational: Translational - rotational: Rotational - spring: flag for joint springs on - damper: flag for joint dampers on - parent_id: identifying number for parent Body{T} - child_id: identifying number for child Body{T} - minimal_index: indices for minimal coordinates - impulses: joint impulses that maintain constraint between two Body{T} objects -""" -mutable struct JointConstraint{T,N,Nc,TJ,RJ} <: Constraint{T,N} - # ID - id::Int64 - name::Symbol - - # joint constraints - translational::TJ - rotational::RJ - - # springs and dampers - spring::Bool - damper::Bool - - # neighbor IDs - parent_id::Int - child_id::Int - - # indices - minimal_index::SVector{Nc,SVector{2,Int64}} # indices for minimal coordinates, assumes joints # Nc = 2 THIS IS SPECIAL CASED - - # impulses - impulses::Vector{SVector{N,T}} - - function JointConstraint(data; - name::Symbol=Symbol("joint_" * randstring(4))) - - @assert data[1][2] == data[2][2] # check parent ids - @assert data[1][3] == data[2][3] # check child ids - - # joints - translational = data[1][1] - rotational = data[2][1] - - # IDs - parent_id = data[1][2] - child_id = data[1][3] - - # data dype - T = typeof(data[1][1]).parameters[1] - - # set springs & dampers off - spring = false - damper = false - - minimal_index = Vector{Int64}[] - N = 0 - for joint_data in data - joint = joint_data[1] - - # set spring & damper on - joint.spring != 0 && (spring = true) - joint.damper != 0 && (damper = true) - - # minimal-coordaintes indices - Nλ = joint_length(joint) - Nset = impulses_length(joint) - if isempty(minimal_index) - push!(minimal_index, [1;3-Nλ]) - else - push!(minimal_index, [last(minimal_index)[2]+1; last(minimal_index)[2]+3-Nλ]) - end - N += Nset - end - - Nc = 2 - impulses = [zeros(T, N) for i=1:2] - - return new{T,N,Nc,typeof(translational),typeof(rotational)}(getGlobalID(), name, translational, rotational, spring, damper, parent_id, child_id, minimal_index, impulses) - end -end - -function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::JointConstraint) - summary(io, constraint) - println(io, "") - println(io, "id: "*string(constraint.id)) - println(io, "name: "*string(constraint.name)) - println(io, "spring: "*string(constraint.spring)) - println(io, "damper: "*string(constraint.damper)) - println(io, "parent_id: "*string(constraint.parent_id)) - println(io, "child_id: "*string(constraint.child_id)) - println(io, "minimal_index: "*string(constraint.minimal_index)) - println(io, "impulses: "*string(constraint.impulses)) -end - -# constraints -# @generated function constraint(mechanism, joint::JointConstraint) -# pbody = :(get_body(mechanism, joint.parent_id)) -# cbody = :(get_body(mechanism, joint.child_id)) -# tra = :(constraint(joint.translational, -# $pbody, $cbody, -# joint.impulses[2][joint_impulse_index(joint,1)], mechanism.μ, mechanism.timestep)) -# rot = :(constraint(joint.rotational, -# $pbody, $cbody, -# joint.impulses[2][joint_impulse_index(joint,2)], mechanism.μ, mechanism.timestep)) -# return :(svcat($tra, $rot)) -# end - function constraint(mechanism, joint::JointConstraint) pbody = get_body(mechanism, joint.parent_id) cbody = get_body(mechanism, joint.child_id) @@ -119,12 +6,6 @@ function constraint(mechanism, joint::JointConstraint) return svcat(tra, rot) end -# # constraints Jacobians -# @generated function constraint_jacobian(joint::JointConstraint) -# tra = :(constraint_jacobian(joint.translational, joint.impulses[2][joint_impulse_index(joint, 1)])) -# rot = :(constraint_jacobian(joint.rotational, joint.impulses[2][joint_impulse_index(joint, 2)])) -# return :(cat($tra, $rot, dims=(1,2))) -# end function constraint_jacobian(joint::JointConstraint) tra = constraint_jacobian(joint.translational, joint.impulses[2][joint_impulse_index(joint, 1)]) rot = constraint_jacobian(joint.rotational, joint.impulses[2][joint_impulse_index(joint, 2)]) @@ -146,64 +27,6 @@ end return :(vcat($tra, $rot)) end -# impulses -function impulses!(mechanism, body::Body, joint::JointConstraint{T,Nλ}) where {T,Nλ} - (Nλ > 0) && (body.state.d -= impulse_map(mechanism, joint, body) * joint.impulses[2]) - joint.spring && (body.state.d -= spring_impulses(mechanism, joint, body)) - joint.damper && (body.state.d -= damper_impulses(mechanism, joint, body)) - return -end - -function impulse_map(mechanism, joint::JointConstraint, body::Body) - relative = body.id == joint.parent_id ? :parent : :child - pbody = get_body(mechanism, joint.parent_id) - cbody = get_body(mechanism, joint.child_id) - tra = impulse_map(relative, joint.translational, - pbody, cbody, - joint.impulses[2][joint_impulse_index(joint, 1)]) - rot = impulse_map(relative, joint.rotational, - pbody, cbody, - joint.impulses[2][joint_impulse_index(joint, 2)]) - return hcat(tra, rot) -end - -# @generated function impulse_map(mechanism, joint::JointConstraint, body::Body) -# relative = :(body.id == joint.parent_id ? :parent : :child) -# pbody = :(get_body(mechanism, joint.parent_id)) -# cbody = :(get_body(mechanism, joint.child_id)) -# tra = :(impulse_map($relative, joint.translational, -# $pbody, $cbody, -# joint.impulses[2][joint_impulse_index(joint, 1)])) -# rot = :(impulse_map($relative, joint.rotational, -# $pbody, $cbody, -# joint.impulses[2][joint_impulse_index(joint, 2)])) -# return :(hcat($tra, $rot)) -# end - -# impulses Jacobians -function impulses_jacobian_velocity!(mechanism, body::Body, joint::JointConstraint) - - # relative - relative = (body.id == joint.parent_id ? :parent : (body.id == joint.child_id ? :child : error())) - - # time step - timestep= mechanism.timestep - - # bodies - pbody = get_body(mechanism, joint.parent_id) - cbody = get_body(mechanism, joint.child_id) - - # springs - joint.spring && (body.state.D -= spring_jacobian_velocity(relative, relative, joint.translational, pbody, cbody, timestep)) - joint.spring && (body.state.D -= spring_jacobian_velocity(relative, relative, joint.rotational, pbody, cbody, timestep)) - - # dampers - joint.damper && (body.state.D -= damper_jacobian_velocity(relative, relative, joint.translational, pbody, cbody, timestep)) - joint.damper && (body.state.D -= damper_jacobian_velocity(relative, relative, joint.rotational, pbody, cbody, timestep)) - - return -end - # off-diagonal Jacobians function off_diagonal_jacobians(mechanism, body::Body, joint::JointConstraint) return -impulse_map(mechanism, joint, body), constraint_jacobian_configuration(mechanism, joint, body) * integrator_jacobian_velocity(body, mechanism.timestep) @@ -298,145 +121,6 @@ function set_matrix_vector_entries!(mechanism, matrix_entry::Entry, vector_entry vector_entry.value = -constraint(mechanism, joint) end -# springs -function spring_impulses(mechanism, joint::JointConstraint{T}, body::Body{T}; - unitary::Bool=false) where T - - relative = (body.id == joint.parent_id ? :parent : :child) - impulses = szeros(T,6) - - pbody = get_body(mechanism, joint.parent_id) - cbody = get_body(mechanism, joint.child_id) - - impulses += spring_impulses(relative, joint.translational, - pbody, - cbody, - mechanism.timestep, - unitary=unitary) - - impulses += spring_impulses(relative, joint.rotational, - pbody, - cbody, - mechanism.timestep, - unitary=unitary) - - return impulses -end - -# dampers -function damper_impulses(mechanism, joint::JointConstraint{T}, body::Body; - unitary::Bool=false) where T - - relative = (body.id == joint.parent_id ? :parent : :child) - impulses = szeros(T,6) - - pbody = get_body(mechanism, joint.parent_id) - cbody = get_body(mechanism, joint.child_id) - - impulses += damper_impulses(relative, joint.translational, - pbody, - cbody, - mechanism.timestep, - unitary=unitary) - - impulses += damper_impulses(relative, joint.rotational, - pbody, - cbody, - mechanism.timestep, - unitary=unitary) - - return impulses -end - -# inputs -function set_input!(joint::JointConstraint{T,N,Nc}, input::AbstractVector) where {T,N,Nc} - @assert length(input) == input_dimension(joint) - # translational - r_idx = SUnitRange(joint.minimal_index[1][1], joint.minimal_index[1][2]) - length(r_idx) > 0 && set_input!(joint.translational, input[r_idx]) - # rotational - r_idx = SUnitRange(joint.minimal_index[2][1], joint.minimal_index[2][2]) - length(r_idx) > 0 && set_input!(joint.rotational, input[r_idx]) - return -end - -function add_input!(joint::JointConstraint{T,N,Nc}, input::AbstractVector) where {T,N,Nc} - @assert length(input) == input_dimension(joint) - add_input!(joint.translational, input[SUnitRange(joint.minimal_index[1][1], joint.minimal_index[1][2])]) - add_input!(joint.rotational, input[SUnitRange(joint.minimal_index[2][1], joint.minimal_index[2][2])]) - return -end - -@generated function input_jacobian_control(mechanism, joint::JointConstraint{T,N,Nc}, body::Body) where {T,N,Nc} - relative = :(body.id == joint.parent_id ? :parent : :child) - pbody = :(get_body(mechanism, joint.parent_id)) - cbody = :(get_body(mechanism, joint.child_id)) - rot = :(input_jacobian_control($relative, joint.translational, $pbody, $cbody, mechanism.input_scaling)) - tra = :(input_jacobian_control($relative, joint.rotational, $pbody, $cbody, mechanism.input_scaling)) - return :(hcat($rot, $tra)) -end - -function input_impulse!(joint::JointConstraint{T,N,Nc}, mechanism, clear::Bool=true) where {T,N,Nc} - pbody = get_body(mechanism, joint.parent_id) - cbody = get_body(mechanism, joint.child_id) - input_impulse!(joint.translational, pbody, cbody, mechanism.input_scaling, clear) - input_impulse!(joint.rotational, pbody, cbody, mechanism.input_scaling, clear) - return -end - -# minimal -@generated function minimal_coordinates(mechanism, joint::JointConstraint{T,N,Nc}) where {T,N,Nc} - pbody = :(get_body(mechanism, joint.parent_id)) - cbody = :(get_body(mechanism, joint.child_id)) - tra = :(minimal_coordinates(joint.translational, $pbody, $cbody)) - rot = :(minimal_coordinates(joint.rotational, $pbody, $cbody)) - return :(svcat($tra, $rot)) -end - -@generated function minimal_velocities(mechanism, joint::JointConstraint{T,N,Nc}) where {T,N,Nc} - pbody = :(get_body(mechanism, joint.parent_id)) - cbody = :(get_body(mechanism, joint.child_id)) - tra = :(minimal_velocities(joint.translational, $pbody, $cbody, mechanism.timestep)) - rot = :(minimal_velocities(joint.rotational, $pbody, $cbody, mechanism.timestep)) - return :(svcat($tra, $rot)) -end - -################################################################################ -# Utilities -################################################################################ -function get_joint_impulses(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,Nc} - n1 = 1 - for j = 1:i-1 - n1 += impulses_length((joint.translational, joint.rotational)[j]) - end - n2 = n1 - 1 + impulses_length((joint.translational, joint.rotational)[i]) - - λi = SVector{n2-n1+1,T}(joint.impulses[2][SUnitRange(n1,n2)]) - return λi -end - -function joint_impulse_index(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,Nc} - s = 0 - for j = 1:i-1 - element = (joint.translational, joint.rotational)[j] - s += impulses_length(element) - end - joint_impulse_index((joint.translational, joint.rotational)[i], s) -end - -# function reset!(joint::JointConstraint{T,N,Nc}; -# scale::T=1.0) where {T,N,Nc} -# λ = [] -# for (i, element) in enumerate((joint.translational, joint.rotational)) -# Nλ = joint_length(element) -# Nb = limits_length(element) -# push!(λ, [scale * sones(2Nb); szeros(Nλ)]) -# end -# joint.impulses[1] = vcat(λ...) -# joint.impulses[2] = vcat(λ...) -# return -# end - function reset!(joint::JointConstraint{T,N,Nc}; scale::T=1.0) where {T,N,Nc} Nλ_tra = joint_length(joint.translational) Nb_tra = limits_length(joint.translational) @@ -446,11 +130,3 @@ function reset!(joint::JointConstraint{T,N,Nc}; scale::T=1.0) where {T,N,Nc} joint.impulses[2] = [scale * sones(2Nb_tra); szeros(Nλ_tra); scale * sones(2Nb_rot); szeros(Nλ_rot)] return end - -function input_dimension(joint::JointConstraint{T,N,Nc}; - ignore_floating_base::Bool=false) where {T,N,Nc} - ignore_floating_base && (N == 0) && return 0 - N̄ = 0 - N̄ = input_dimension(joint.translational) + input_dimension(joint.rotational) - return N̄ -end diff --git a/src/joints/constructor.jl b/src/joints/constructor.jl new file mode 100644 index 000000000..ffa821260 --- /dev/null +++ b/src/joints/constructor.jl @@ -0,0 +1,99 @@ +""" + JointConstraint{T} <: Constraint{T} + + constraint restricting translational and rotational degrees of freedom between two Body objects. + + id: a unique identifying number + name: a unique identifying name + translational: Translational + rotational: Rotational + spring: flag for joint springs on + damper: flag for joint dampers on + parent_id: identifying number for parent Body{T} + child_id: identifying number for child Body{T} + minimal_index: indices for minimal coordinates + impulses: joint impulses that maintain constraint between two Body{T} objects +""" +mutable struct JointConstraint{T,N,Nc,TJ,RJ} <: Constraint{T,N} + # ID + id::Int64 + name::Symbol + + # joint constraints + translational::TJ + rotational::RJ + + # springs and dampers + spring::Bool + damper::Bool + + # neighbor IDs + parent_id::Int + child_id::Int + + # indices + minimal_index::SVector{Nc,SVector{2,Int64}} # indices for minimal coordinates, assumes joints # Nc = 2 THIS IS SPECIAL CASED + + # impulses + impulses::Vector{SVector{N,T}} + + function JointConstraint(data; + name::Symbol=Symbol("joint_" * randstring(4))) + + @assert data[1][2] == data[2][2] # check parent ids + @assert data[1][3] == data[2][3] # check child ids + + # joints + translational = data[1][1] + rotational = data[2][1] + + # IDs + parent_id = data[1][2] + child_id = data[1][3] + + # data dype + T = typeof(data[1][1]).parameters[1] + + # set springs & dampers off + spring = false + damper = false + + minimal_index = Vector{Int64}[] + N = 0 + for joint_data in data + joint = joint_data[1] + + # set spring & damper on + joint.spring != 0 && (spring = true) + joint.damper != 0 && (damper = true) + + # minimal-coordaintes indices + Nλ = joint_length(joint) + Nset = impulses_length(joint) + if isempty(minimal_index) + push!(minimal_index, [1;3-Nλ]) + else + push!(minimal_index, [last(minimal_index)[2]+1; last(minimal_index)[2]+3-Nλ]) + end + N += Nset + end + + Nc = 2 + impulses = [zeros(T, N) for i=1:2] + + return new{T,N,Nc,typeof(translational),typeof(rotational)}(getGlobalID(), name, translational, rotational, spring, damper, parent_id, child_id, minimal_index, impulses) + end +end + +function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::JointConstraint) + summary(io, constraint) + println(io, "") + println(io, "id: "*string(constraint.id)) + println(io, "name: "*string(constraint.name)) + println(io, "spring: "*string(constraint.spring)) + println(io, "damper: "*string(constraint.damper)) + println(io, "parent_id: "*string(constraint.parent_id)) + println(io, "child_id: "*string(constraint.child_id)) + println(io, "minimal_index: "*string(constraint.minimal_index)) + println(io, "impulses: "*string(constraint.impulses)) +end diff --git a/src/joints/impulses.jl b/src/joints/impulses.jl index efb4a93cf..5f4be1c30 100644 --- a/src/joints/impulses.jl +++ b/src/joints/impulses.jl @@ -1,3 +1,94 @@ +function impulses!(mechanism, body::Body, joint::JointConstraint{T,Nλ}) where {T,Nλ} + (Nλ > 0) && (body.state.d -= impulse_map(mechanism, joint, body) * joint.impulses[2]) + joint.spring && (body.state.d -= spring_impulses(mechanism, joint, body)) + joint.damper && (body.state.d -= damper_impulses(mechanism, joint, body)) + return +end + +function impulse_map(mechanism, joint::JointConstraint, body::Body) + relative = body.id == joint.parent_id ? :parent : :child + pbody = get_body(mechanism, joint.parent_id) + cbody = get_body(mechanism, joint.child_id) + tra = impulse_map(relative, joint.translational, + pbody, cbody, + joint.impulses[2][joint_impulse_index(joint, 1)]) + rot = impulse_map(relative, joint.rotational, + pbody, cbody, + joint.impulses[2][joint_impulse_index(joint, 2)]) + return hcat(tra, rot) +end + +function impulses_jacobian_velocity!(mechanism, body::Body, joint::JointConstraint) + + # relative + relative = (body.id == joint.parent_id ? :parent : (body.id == joint.child_id ? :child : error())) + + # time step + timestep= mechanism.timestep + + # bodies + pbody = get_body(mechanism, joint.parent_id) + cbody = get_body(mechanism, joint.child_id) + + # springs + joint.spring && (body.state.D -= spring_jacobian_velocity(relative, relative, joint.translational, pbody, cbody, timestep)) + joint.spring && (body.state.D -= spring_jacobian_velocity(relative, relative, joint.rotational, pbody, cbody, timestep)) + + # dampers + joint.damper && (body.state.D -= damper_jacobian_velocity(relative, relative, joint.translational, pbody, cbody, timestep)) + joint.damper && (body.state.D -= damper_jacobian_velocity(relative, relative, joint.rotational, pbody, cbody, timestep)) + + return +end + +function spring_impulses(mechanism, joint::JointConstraint{T}, body::Body{T}; + unitary::Bool=false) where T + + relative = (body.id == joint.parent_id ? :parent : :child) + impulses = szeros(T,6) + + pbody = get_body(mechanism, joint.parent_id) + cbody = get_body(mechanism, joint.child_id) + + impulses += spring_impulses(relative, joint.translational, + pbody, + cbody, + mechanism.timestep, + unitary=unitary) + + impulses += spring_impulses(relative, joint.rotational, + pbody, + cbody, + mechanism.timestep, + unitary=unitary) + + return impulses +end + +function damper_impulses(mechanism, joint::JointConstraint{T}, body::Body; + unitary::Bool=false) where T + + relative = (body.id == joint.parent_id ? :parent : :child) + impulses = szeros(T,6) + + pbody = get_body(mechanism, joint.parent_id) + cbody = get_body(mechanism, joint.child_id) + + impulses += damper_impulses(relative, joint.translational, + pbody, + cbody, + mechanism.timestep, + unitary=unitary) + + impulses += damper_impulses(relative, joint.rotational, + pbody, + cbody, + mechanism.timestep, + unitary=unitary) + + return impulses +end + ################################################################################ # Impulse Transform ################################################################################ @@ -19,3 +110,26 @@ function impulse_map_jacobian(relative::Symbol, jacobian::Symbol, joint::Joint, current_configuration(cbody.state)..., p) end + +################################################################################ +# Utilities +################################################################################ +function get_joint_impulses(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,Nc} + n1 = 1 + for j = 1:i-1 + n1 += impulses_length((joint.translational, joint.rotational)[j]) + end + n2 = n1 - 1 + impulses_length((joint.translational, joint.rotational)[i]) + + λi = SVector{n2-n1+1,T}(joint.impulses[2][SUnitRange(n1,n2)]) + return λi +end + +function joint_impulse_index(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,Nc} + s = 0 + for j = 1:i-1 + element = (joint.translational, joint.rotational)[j] + s += impulses_length(element) + end + joint_impulse_index((joint.translational, joint.rotational)[i], s) +end \ No newline at end of file diff --git a/src/joints/input.jl b/src/joints/input.jl new file mode 100644 index 000000000..ef13c35ad --- /dev/null +++ b/src/joints/input.jl @@ -0,0 +1,42 @@ +function set_input!(joint::JointConstraint{T,N,Nc}, input::AbstractVector) where {T,N,Nc} + @assert length(input) == input_dimension(joint) + # translational + r_idx = SUnitRange(joint.minimal_index[1][1], joint.minimal_index[1][2]) + length(r_idx) > 0 && set_input!(joint.translational, input[r_idx]) + # rotational + r_idx = SUnitRange(joint.minimal_index[2][1], joint.minimal_index[2][2]) + length(r_idx) > 0 && set_input!(joint.rotational, input[r_idx]) + return +end + +function add_input!(joint::JointConstraint{T,N,Nc}, input::AbstractVector) where {T,N,Nc} + @assert length(input) == input_dimension(joint) + add_input!(joint.translational, input[SUnitRange(joint.minimal_index[1][1], joint.minimal_index[1][2])]) + add_input!(joint.rotational, input[SUnitRange(joint.minimal_index[2][1], joint.minimal_index[2][2])]) + return +end + +@generated function input_jacobian_control(mechanism, joint::JointConstraint{T,N,Nc}, body::Body) where {T,N,Nc} + relative = :(body.id == joint.parent_id ? :parent : :child) + pbody = :(get_body(mechanism, joint.parent_id)) + cbody = :(get_body(mechanism, joint.child_id)) + rot = :(input_jacobian_control($relative, joint.translational, $pbody, $cbody, mechanism.input_scaling)) + tra = :(input_jacobian_control($relative, joint.rotational, $pbody, $cbody, mechanism.input_scaling)) + return :(hcat($rot, $tra)) +end + +function input_impulse!(joint::JointConstraint{T,N,Nc}, mechanism, clear::Bool=true) where {T,N,Nc} + pbody = get_body(mechanism, joint.parent_id) + cbody = get_body(mechanism, joint.child_id) + input_impulse!(joint.translational, pbody, cbody, mechanism.input_scaling, clear) + input_impulse!(joint.rotational, pbody, cbody, mechanism.input_scaling, clear) + return +end + +function input_dimension(joint::JointConstraint{T,N,Nc}; + ignore_floating_base::Bool=false) where {T,N,Nc} + ignore_floating_base && (N == 0) && return 0 + N̄ = 0 + N̄ = input_dimension(joint.translational) + input_dimension(joint.rotational) + return N̄ +end \ No newline at end of file diff --git a/src/joints/minimal.jl b/src/joints/minimal.jl index 8594a7656..936986834 100644 --- a/src/joints/minimal.jl +++ b/src/joints/minimal.jl @@ -1,3 +1,20 @@ +# minimal +@generated function minimal_coordinates(mechanism, joint::JointConstraint{T,N,Nc}) where {T,N,Nc} + pbody = :(get_body(mechanism, joint.parent_id)) + cbody = :(get_body(mechanism, joint.child_id)) + tra = :(minimal_coordinates(joint.translational, $pbody, $cbody)) + rot = :(minimal_coordinates(joint.rotational, $pbody, $cbody)) + return :(svcat($tra, $rot)) +end + +@generated function minimal_velocities(mechanism, joint::JointConstraint{T,N,Nc}) where {T,N,Nc} + pbody = :(get_body(mechanism, joint.parent_id)) + cbody = :(get_body(mechanism, joint.child_id)) + tra = :(minimal_velocities(joint.translational, $pbody, $cbody, mechanism.timestep)) + rot = :(minimal_velocities(joint.rotational, $pbody, $cbody, mechanism.timestep)) + return :(svcat($tra, $rot)) +end + ################################################################################ # Coordinates ################################################################################ From a97f1139673a3ccb7b7932c0d04d7fdbd4ff72bd Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 26 Jun 2023 14:00:48 +0200 Subject: [PATCH 6/7] Working impact version with DiffColl.jl --- Project.toml | 2 + contact_test.jl | 84 ++++++++ src/Dojo.jl | 7 +- src/bodies/shapes.jl | 56 +++++- src/contacts/collisions/general_collision.jl | 200 +++++++++++++++++++ src/contacts/constraints.jl | 4 +- src/contacts/contact.jl | 10 +- src/contacts/impact.jl | 29 +++ src/integrators/constraint.jl | 2 +- src/solver/mehrotra.jl | 2 +- test/jacobian.jl | 6 - 11 files changed, 378 insertions(+), 24 deletions(-) create mode 100644 contact_test.jl create mode 100644 src/contacts/collisions/general_collision.jl diff --git a/Project.toml b/Project.toml index 7d7bdeb3d..dc2ee35c5 100644 --- a/Project.toml +++ b/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" diff --git a/contact_test.jl b/contact_test.jl new file mode 100644 index 000000000..667595151 --- /dev/null +++ b/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, gravity=0) + +# 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) diff --git a/src/Dojo.jl b/src/Dojo.jl index 2bf8a1c92..3bb7469ff 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -24,6 +24,8 @@ using Meshing using GeometryBasics using GraphBasedSystems using CoordinateTransformations +using DifferentiableCollisions +using DifferentiableCollisions: proximity, AbstractPrimitive using DocStringExtensions @@ -52,7 +54,7 @@ include(joinpath("bodies", "origin.jl")) include(joinpath("bodies", "set.jl")) # Mechanism -include(joinpath("joints", "constraints.jl")) +include(joinpath("joints", "constructor.jl")) include(joinpath("contacts", "constructor.jl")) include(joinpath("contacts", "contact.jl")) @@ -76,6 +78,8 @@ include(joinpath("mechanics", "momentum.jl")) include(joinpath("mechanics", "energy.jl")) # Joints +include(joinpath("joints", "constraints.jl")) +include(joinpath("joints", "input.jl")) include(joinpath("joints", "orthogonal.jl")) include(joinpath("joints", "joint.jl")) include(joinpath("joints", "translational", "constructor.jl")) @@ -103,6 +107,7 @@ include(joinpath("contacts", "collisions", "point_to_segment.jl")) include(joinpath("contacts", "collisions", "point_to_box_v2.jl")) include(joinpath("contacts", "collisions", "sphere_halfspace.jl")) include(joinpath("contacts", "collisions", "sphere_sphere.jl")) +include(joinpath("contacts", "collisions", "general_collision.jl")) include(joinpath("contacts", "collisions", "sphere_capsule.jl")) include(joinpath("contacts", "collisions", "sphere_box.jl")) include(joinpath("contacts", "collisions", "string.jl")) diff --git a/src/bodies/shapes.jl b/src/bodies/shapes.jl index b3ed5f641..6e9c3a81d 100644 --- a/src/bodies/shapes.jl +++ b/src/bodies/shapes.jl @@ -65,6 +65,7 @@ end color: RGBA """ mutable struct Box{T} <: Shape{T} + primitive::AbstractPrimitive position_offset::SVector{3,T} orientation_offset::Quaternion{T} xyz::SVector{3,T} @@ -77,7 +78,7 @@ mutable struct Box{T} <: Shape{T} scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((x, y, z, position_offset, orientation_offset))...) - new{T}(position_offset, orientation_offset, [x; y; z], scale, color) + new{T}(box_primitive(T(x),T(y),T(z)),position_offset, orientation_offset, [x; y; z], scale, color) end function Box(x::Real, y::Real, z::Real, m::Real; @@ -88,7 +89,7 @@ mutable struct Box{T} <: Shape{T} color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((x, y, z, m, position_offset, orientation_offset))...) J = 1 / 12 * m * diagm([y^2 + z^2; x^2 + z^2; x^2 + y^2]) - return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, [x;y;z], scale, color)) + return Body(m, J; name=name, shape=new{T}(box_primitive(T(x),T(y),T(z)),position_offset, orientation_offset, [x;y;z], scale, color)) end end @@ -104,6 +105,7 @@ end color: RGBA """ mutable struct Cylinder{T} <: Shape{T} + primitive::AbstractPrimitive position_offset::SVector{3,T} orientation_offset::Quaternion{T} rh::SVector{2,T} @@ -117,7 +119,7 @@ mutable struct Cylinder{T} <: Shape{T} scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((r, h, position_offset, orientation_offset))...) - new{T}(position_offset, orientation_offset, [r;h], scale, color) + new{T}(cylinder_primitive(T(r),T(h)),position_offset, orientation_offset, [r;h], scale, color) end function Cylinder(r::Real, h::Real, m::Real; @@ -128,7 +130,7 @@ mutable struct Cylinder{T} <: Shape{T} color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((r, h, m, position_offset, orientation_offset))...) J = 1 / 2 * m * diagm([r^2 + 1 / 6 * h^2; r^2 + 1 / 6 * h^2; r^2]) - return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, [r;h], scale, color)) + return Body(m, J; name=name, shape=new{T}(cylinder_primitive(T(r),T(h)),position_offset, orientation_offset, [r;h], scale, color)) end end @@ -229,6 +231,7 @@ end color: RGBA """ mutable struct Sphere{T} <: Shape{T} + primitive::AbstractPrimitive position_offset::SVector{3,T} orientation_offset::Quaternion{T} r::T @@ -241,7 +244,7 @@ mutable struct Sphere{T} <: Shape{T} scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((r, position_offset, orientation_offset))...) - new{T}(position_offset, orientation_offset, r, scale, color) + new{T}(sphere_primitive(T(r)),position_offset, orientation_offset, r, scale, color) end function Sphere(r::Real, m::Real; @@ -252,7 +255,7 @@ mutable struct Sphere{T} <: Shape{T} color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((r, m, position_offset, orientation_offset))...) J = 2 / 5 * m * diagm([r^2 for i = 1:3]) - return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, r, scale, color)) + return Body(m, J; name=name, shape=new{T}(sphere_primitive(T(r)),position_offset, orientation_offset, r, scale, color)) end end @@ -268,6 +271,7 @@ end color: RGBA """ mutable struct Pyramid{T} <: Shape{T} + primitive::AbstractPrimitive position_offset::SVector{3,T} orientation_offset::Quaternion{T} wh::SVector{2,T} @@ -281,7 +285,7 @@ mutable struct Pyramid{T} <: Shape{T} scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((w, h, position_offset, orientation_offset))...) - new{T}(position_offset, orientation_offset, [w;h], scale, color) + new{T}(pyramid_primitive(T(w),T(h)),position_offset, orientation_offset, [w;h], scale, color) end function Pyramid(w::Real, h::Real, m::Real; @@ -291,7 +295,7 @@ mutable struct Pyramid{T} <: Shape{T} name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((w, h, m, position_offset, orientation_offset))...) J = 1/80 * m * diagm([4*w^2+3*h^2;4*w^2+3*h^2;8*w^2]) - return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, [w;h], scale, color)) + return Body(m, J; name=name, shape=new{T}(pyramid_primitive(T(w),T(h)),position_offset, orientation_offset, [w;h], scale, color)) end end @@ -340,6 +344,42 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, shape::Shape) println(io," color: "*string(shape.color)) end +# TODO position and orientation offset not accounted for +function box_primitive(x,y,z) + A = SA[ + 1 0 0 + 0 1 0 + 0 0 1 + -1 0 0 + 0 -1 0 + 0 0 -1.0 + ] + b = SA[x;y;z;x;y;z]/2 + + return DifferentiableCollisions.Polytope(A,b) +end + +function cylinder_primitive(r,h) + return DifferentiableCollisions.Cylinder(r, h) +end + +function sphere_primitive(r) + return DifferentiableCollisions.Sphere(r) +end + +function pyramid_primitive(w,h) + A = SA[ + 1/(3/8*w) 0 1/(3/4*h) + 0 1/(3/8*w) 1/(3/4*h) + -1/(3/8*w) 0 1/(3/4*h) + 0 -1/(3/8*w) 1/(3/4*h) + 0 0 -1/(1/4*h) + ] + b = SA[1;1;1;1;1.0] + + return DifferentiableCollisions.Polytope(A,b) +end + function convert_shape(box::Box) x, y, z = Tuple(box.xyz) return GeometryBasics.HyperRectangle(Vec(-x / 2.0, -y / 2.0, -z / 2.0), Vec(x, y, z)) diff --git a/src/contacts/collisions/general_collision.jl b/src/contacts/collisions/general_collision.jl new file mode 100644 index 000000000..a4f02a35c --- /dev/null +++ b/src/contacts/collisions/general_collision.jl @@ -0,0 +1,200 @@ +""" + GeneralCollision + + collision between two objects using DifferentiableCollisions + + origin_parent: position of contact on parent body relative to center of mass + origin_child: position of contact on parent body relative to center of mass + radius_parent: radius of contact for parent body + radius_child: radius of contact for child body +""" +mutable struct GeneralCollision{T,O,I,OI} <: Collision{T,O,I,OI} + origin_parent::SVector{I,T} + origin_child::SVector{I,T} + radius_parent::T + radius_child::T + primitive1::AbstractPrimitive + primitive2::AbstractPrimitive + α::T + intersection_point::SVector{3,T} +end + + +# function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, collision::GeneralCollision) +# summary(io, collision) +# println(io, "") +# println(io, "origin_parent: "*string(collision.origin_parent)) +# println(io, "origin_child: "*string(collision.origin_child)) +# println(io, "radius_parent: "*string(collision.radius_parent)) +# println(io, "radius_child: "*string(collision.radius_child)) +# end + +# not really the distance +function distance(collision::GeneralCollision, xp, qp, xc, qc) + primitive1 = collision.primitive1 + primitive2 = collision.primitive2 + + primitive1.r = xp + primitive1.q = Dojo.vector(qp) + primitive2.r = xc + primitive2.q = Dojo.vector(qc) + + return proximity(primitive1,primitive2)[1] - 1 +end + +function ∂distance∂x(gradient::Symbol, collision::GeneralCollision, xp, qp, xc, qc) + # contact origin points + cop = contact_point_origin(xp, qp, collision.origin_parent) + coc = contact_point_origin(xc, qc, collision.origin_child) + + # distance between contact origins + d = norm(cop - coc, 2) + ∂norm∂d = ∂norm∂x(cop - coc) + + if gradient == :parent + D = ∂norm∂d * 1.0 * ∂contact_point_origin∂x(xp, qp, collision.origin_parent) + elseif gradient == :child + D = ∂norm∂d * -1.0 * ∂contact_point_origin∂x(xc, qc, collision.origin_child) + end + + if gradient == :parent + FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, x, qp, xc, qc), xp) + elseif gradient == :child + FD = FiniteDiff.finite_difference_jacobian(x -> distance(collision, xp, qp, x, qc), xc) + end + + return FD + + # @assert norm(D - FD, Inf) < 1.0e-5 + + return D +end + +function ∂distance∂q(gradient::Symbol, collision::GeneralCollision, xp, qp, xc, qc) + # contact origin points + cop = contact_point_origin(xp, qp, collision.origin_parent) + coc = contact_point_origin(xc, qc, collision.origin_child) + + # distance between contact origins + d = norm(cop - coc, 2) + ∂norm∂d = ∂norm∂x(cop - coc) + + if gradient == :parent + D = ∂norm∂d * 1.0 * ∂contact_point_origin∂q(xp, qp, collision.origin_parent) + elseif gradient == :child + D = ∂norm∂d * -1.0 * ∂contact_point_origin∂q(xc, qc, collision.origin_child) + end + + if gradient == :parent + FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, Quaternion(q...), xc, qc), vector(qp)) + elseif gradient == :child + FD = FiniteDiff.finite_difference_jacobian(q -> distance(collision, xp, qp, xc, Quaternion(q...)), vector(qc)) + end + + return FD + # @assert norm(D - FD, Inf) < 1.0e-5 + + return D +end + +# contact point in world frame +function contact_point(relative::Symbol, collision::GeneralCollision, xp, qp, xc, qc) + primitive1 = collision.primitive1 + primitive2 = collision.primitive2 + + primitive1.r = xp + primitive1.q = Dojo.vector(qp) + primitive2.r = xc + primitive2.q = Dojo.vector(qc) + + α, intersection_point = proximity(primitive1,primitive2) + if relative == :parent + return xp + (intersection_point - xp)/α + elseif relative == :child + return xc + (intersection_point - xc)/α + end +end + +function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::GeneralCollision, xp, qp, xc, qc) + # contact origin points + cop = contact_point_origin(xp, qp, collision.origin_parent) + coc = contact_point_origin(xc, qc, collision.origin_child) + + # direction of minimum distance (child to parent) + d = cop - coc + dir = normalize(d) + + if relative == :parent + # cop - collision.radius_parent * dir + if jacobian == :parent + ∂c∂x = ∂contact_point_origin∂x(xp, qp, collision.origin_parent) + X = ∂c∂x + X -= collision.radius_parent * ∂normalize∂x(d) * ∂c∂x + elseif jacobian == :child + X = -1.0 * collision.radius_parent * ∂normalize∂x(d) * -1.0 * ∂contact_point_origin∂x(xc, qc, collision.origin_child) + end + elseif relative == :child + # coc + collision.radius_child * dir + if jacobian == :parent + X = 1.0 * collision.radius_child * ∂normalize∂x(d) * ∂contact_point_origin∂x(xp, qp, collision.origin_parent) + elseif jacobian == :child + ∂c∂x = ∂contact_point_origin∂x(xc, qc, collision.origin_child) + X = ∂c∂x + X += collision.radius_child * ∂normalize∂x(d) * -1.0 * ∂c∂x + end + end + + if jacobian == :parent + FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, x, qp, xc, qc), xp) + elseif jacobian == :child + FD = FiniteDiff.finite_difference_jacobian(x -> contact_point(relative, collision, xp, qp, x, qc), xc) + end + + return FD + # @assert norm(X - FD, Inf) < 1.0e-5 + + return X +end + +function ∂contact_point∂q(relative::Symbol, jacobian::Symbol, collision::GeneralCollision, xp, qp, xc, qc) + # contact origin points + cop = contact_point_origin(xp, qp, collision.origin_parent) + coc = contact_point_origin(xc, qc, collision.origin_child) + + # direction of minimum distance (child to parent) + d = cop - coc + dir = normalize(d) + + if relative == :parent + # cop - collision.radius_parent * dir + if jacobian == :parent + ∂c∂q = ∂contact_point_origin∂q(xp, qp, collision.origin_parent) + Q = ∂c∂q + Q -= collision.radius_parent * ∂normalize∂x(d) * ∂c∂q + elseif jacobian == :child + Q = -1.0 * collision.radius_parent * ∂normalize∂x(d) * -1.0 * ∂contact_point_origin∂q(xc, qc, collision.origin_child) + end + elseif relative == :child + # coc + collision.radius_child * dir + if jacobian == :parent + Q = 1.0 * collision.radius_child * ∂normalize∂x(d) * ∂contact_point_origin∂q(xp, qp, collision.origin_parent) + elseif jacobian == :child + ∂c∂q = ∂contact_point_origin∂q(xc, qc, collision.origin_child) + Q = ∂c∂q + Q += collision.radius_child * ∂normalize∂x(d) * -1.0 * ∂c∂q + end + end + + if jacobian == :parent + FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, Quaternion(q...), xc, qc), vector(qp)) + elseif jacobian == :child + FD = FiniteDiff.finite_difference_jacobian(q -> contact_point(relative, collision, xp, qp, xc, Quaternion(q...)), vector(qc)) + end + + return FD + + # @assert norm(Q - FD, Inf) < 1.0e-5 + + return Q +end + diff --git a/src/contacts/constraints.jl b/src/contacts/constraints.jl index 5849efd6c..5ba230c6a 100644 --- a/src/contacts/constraints.jl +++ b/src/contacts/constraints.jl @@ -37,7 +37,7 @@ function impulse_map(mechanism, contact::ContactConstraint, body::Body) relative = (body.id == contact.parent_id ? :parent : :child) pbody = get_body(mechanism, contact.parent_id) cbody = get_body(mechanism, contact.child_id) - return impulse_map(relative, contact.model, pbody, cbody, mechanism.timestep) + return impulse_map(relative, contact.model, pbody, cbody) end function impulse_map_jacobian_configuration(mechanism, body::Body{T}, contact::ContactConstraint{T}) where T @@ -52,7 +52,7 @@ end function impulses_jacobian_velocity!(mechanism, body::Body, contact::ContactConstraint) timestep = mechanism.timestep - body.state.D -= impulse_map_jacobian_configuration(mechanism, body, contact) * integrator_jacobian_velocity(body, timestep) + # body.state.D -= impulse_map_jacobian_configuration(mechanism, body, contact) * integrator_jacobian_velocity(body, timestep) return end diff --git a/src/contacts/contact.jl b/src/contacts/contact.jl index 256f743fb..8878ff790 100644 --- a/src/contacts/contact.jl +++ b/src/contacts/contact.jl @@ -76,10 +76,10 @@ function constraint_jacobian_velocity(relative::Symbol, model::Contact, return [V Ω] end -function impulse_map(relative::Symbol, model::Contact, pbody::Node, cbody::Node, timestep) +function impulse_map(relative::Symbol, model::Contact, pbody::Node, cbody::Node) # configurations - xp, qp = next_configuration(pbody.state, timestep) - xc, qc = next_configuration(cbody.state, timestep) + xp, qp = current_configuration(pbody.state) + xc, qc = current_configuration(cbody.state) # mapping X = force_mapping(relative, model, xp, qp, xc, qc) @@ -102,8 +102,8 @@ end function impulse_map_jacobian(relative::Symbol, jacobian::Symbol, model::Contact, pbody::Node, cbody::Node, λ, timestep) # configurations - xp, qp = next_configuration(pbody.state, timestep) - xc, qc = next_configuration(cbody.state, timestep) + xp, qp = current_configuration(pbody.state) + xc, qc = current_configuration(cbody.state) # mapping X = force_mapping(relative, model, xp, qp, xc, qc) diff --git a/src/contacts/impact.jl b/src/contacts/impact.jl index eb80fbf8e..012d7ebcc 100644 --- a/src/contacts/impact.jl +++ b/src/contacts/impact.jl @@ -52,6 +52,35 @@ function ImpactContact(bodies::AbstractVector{Body{T}}, normals::AbstractVector{ [ImpactContact(bodies[i], normals[i]; contact_origin=contact_origins[i], contact_radius=contact_radii[i]) for i in eachindex(bodies)] end +function ImpactContact(body1::Body{T}, body2::Body{T}) where T + # friction parametrization + parameterization = szeros(T, 0, 2) + + origin_parent = szeros(3) + origin_child = szeros(3) + radius_parent = 1 + radius_child = 1 + primitive1 = body1.shape.primitive + primitive2 = body2.shape.primitive + α = 0 + intersection_point = szeros(3) + + collision = GeneralCollision{T,0,3,0}(origin_parent,origin_child,radius_parent,radius_child,primitive1,primitive2,α,intersection_point) + + ImpactContact{Float64,2}(parameterization, collision), body1.id, body2.id +end + +function ImpactContact(bodies::AbstractVector{Body{T}}) where T + impacts = Tuple[] + for i=1:length(bodies) + for j=i+1:length(bodies) + impacts = [impacts;ImpactContact(bodies[i],bodies[j])] + end + end + + return impacts +end + function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs<:ImpactContact{T,N}} # contact model model = contact.model diff --git a/src/integrators/constraint.jl b/src/integrators/constraint.jl index 4a799df2f..0153b0ef8 100644 --- a/src/integrators/constraint.jl +++ b/src/integrators/constraint.jl @@ -27,7 +27,7 @@ function constraint(mechanism::Mechanism{T,Nn,Ne,Nb}, body::Body{T}) where {T,Nn # impulses for id in connections(mechanism.system, body.id) Ne < id <= Ne + Nb && continue # body - impulses!(mechanism, body, get_node(mechanism, id)) + impulses!(mechanism, body, get_node(mechanism, id)) # TODO there is something off with the jacobians. Seems like some incorrect timesteps are used. end return state.d diff --git a/src/solver/mehrotra.jl b/src/solver/mehrotra.jl index d9e5c5b1f..25845b144 100644 --- a/src/solver/mehrotra.jl +++ b/src/solver/mehrotra.jl @@ -31,7 +31,7 @@ function mehrotra!(mechanism::Mechanism{T}; opts=SolverOptions{T}()) where T (n == opts.max_iter) && (opts.verbose && (@warn "failed mehrotra")) # affine search direction - μ = 0.0 + μ = 0.0 # TODO is this cause of a bug? pull_residual!(mechanism) # store the residual inside mechanism.residual_entries ldu_factorization!(mechanism.system) # factorize system, modifies the matrix in place ldu_backsubstitution!(mechanism.system) # solve system, modifies the vector in place diff --git a/test/jacobian.jl b/test/jacobian.jl index 586e8660d..518579fb2 100644 --- a/test/jacobian.jl +++ b/test/jacobian.jl @@ -23,11 +23,8 @@ function test_solmat(model; opts=SolverOptions(rtol=ϵ, btol=ϵ)) # Set data - Nb = length(mechanism.bodies) data = Dojo.get_data(mechanism) - Dojo.set_data!(mechanism, data) sol = Dojo.get_solution(mechanism) - attjac = Dojo.attitude_jacobian(data, Nb) # IFT solmat = Dojo.full_matrix(mechanism.system) @@ -47,9 +44,6 @@ function finite_difference_solution_matrix(mechanism::Mechanism, data::AbstractV nsol = length(sol) jac = zeros(nsol, nsol) - Dojo.set_data!(mechanism, data) - Dojo.set_solution!(mechanism, sol) - for i = 1:nsol verbose && println("$i / $nsol") solp = deepcopy(sol) From 1bb91c431a09276e812c29bf8dd64c3e6d38ce57 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Mon, 26 Jun 2023 16:30:15 +0200 Subject: [PATCH 7/7] Make Dojo compile --- contact_test.jl | 2 +- contact_test_old.jl | 392 ++++++++++++++++++++++++++++++++++++++ src/Dojo.jl | 12 +- src/contacts/utilities.jl | 29 --- 4 files changed, 399 insertions(+), 36 deletions(-) create mode 100644 contact_test_old.jl diff --git a/contact_test.jl b/contact_test.jl index 667595151..1072e1b1d 100644 --- a/contact_test.jl +++ b/contact_test.jl @@ -57,7 +57,7 @@ joint4 = JointConstraint(Floating(origin, body4)) joints = [joint1;joint2;joint3;joint4] dojo_contacts = ContactConstraint(ImpactContact(bodies)) -mech = Mechanism(origin, bodies, joints, dojo_contacts; timestep=h, gravity=0) +mech = Mechanism(origin, bodies, joints, dojo_contacts; timestep=h) # create the indexing named tuple N_bodies = length(bodies) diff --git a/contact_test_old.jl b/contact_test_old.jl new file mode 100644 index 000000000..a61efdffd --- /dev/null +++ b/contact_test_old.jl @@ -0,0 +1,392 @@ +using Dojo +import DifferentiableCollisions as dc +using LinearAlgebra +using Printf +using StaticArrays +import ForwardDiff as FD +import MeshCat as mc +import Random +using Colors +using SparseArrays +using Combinatorics + +function hat(ω) + return [0 -ω[3] ω[2]; + ω[3] 0 -ω[1]; + -ω[2] ω[1] 0] +end +function L(Q) + [Q[1] -Q[2:4]'; Q[2:4] Q[1]*I + hat(Q[2:4])] +end + +function R(Q) + [Q[1] -Q[2:4]'; Q[2:4] Q[1]*I - hat(Q[2:4])] +end + +function ρ(ϕ) + (1/(sqrt(1 + dot(ϕ,ϕ))))*[1;ϕ] +end + +const H = [zeros(1,3); I]; + +const gravity = [0;0;-9.81] + +const T = Diagonal([1.0; -1; -1; -1]) + +function G(Q) + return L(Q)*H +end +function Expq(ϕ) + # The quaternion exponential map ϕ → q + θ = norm(ϕ) + q = [cos(θ/2); 0.5*ϕ*sinc(θ/(2*pi))] +end + +function trans_part(m,x1,x2,x3,Δt) + # translational DEL (add gravity here if you want) + (1/Δt)*m*(x2-x1) - (1/Δt)*m*(x3-x2) + Δt*m*[0;0;-9.81] +end + +function rot_part(J,q1,q2,q3,Δt) + # rotational DEL + (2.0/Δt)*G(q2)'*L(q1)*H*J*H'*L(q1)'*q2 + (2.0/Δt)*G(q2)'*T*R(q3)'*H*J*H'*L(q2)'*q3 +end + +function single_DEL(z₋,z,z₊,J,m,h) + # translational and rotational DEL together + p₋ = z₋[1:3] + q₋ = z₋[4:7] + p = z[1:3] + q = z[4:7] + p₊ = z₊[1:3] + q₊ = z₊[4:7] + [ + trans_part(m,p₋,p,p₊,h); + rot_part(J,q₋,q,q₊,h) + ] +end +function create_indexing(N_bodies) + # here we create indexing for an arbitrary number of bodies (all in DCOL) + idx_z = [((i - 1)*7 .+ (1:7)) for i = 1:N_bodies] + idx_Δz = [((i - 1)*6 .+ (1:6)) for i = 1:N_bodies] + + interactions = collect(combinations(1:N_bodies,2)) + N_interactions = length(interactions) + @assert N_interactions == binomial(N_bodies,2) + + idx_s = idx_z[end][end] .+ (1:N_interactions) + idx_Δs = idx_s .- N_bodies + idx_λ = idx_s[end][end] .+ (1:N_interactions) + idx_Δλ = idx_λ .- N_bodies + + idx_α = 6*N_bodies .+ (1:N_interactions) + + # throw all the stuff we want in this idx named tuple + idx = ( + z = idx_z, + s = idx_s, + λ = idx_λ, + Δz = idx_Δz, + Δs = idx_Δs, + Δλ = idx_Δλ, + interactions = interactions, + N_interactions = N_interactions, + N_bodies = N_bodies, + α = idx_α + ) + + return idx +end + + +function update_objects!(P, w, idx) + # update position and orientation of each DCOL struct + for i = 1:idx.N_bodies + P[i].r = w[idx.z[i][SA[1,2,3]]] + P[i].q = normalize(w[idx.z[i][SA[4,5,6,7]]]) + end + nothing +end + +function contacts(P,idx) + # contacts between every pair of DCOL bodies + [(dc.proximity(P[i],P[j])[1] - 1) for (i,j) in idx.interactions] +end +function Gbar_2_body(z,idx_z1, idx_z2) + # attitude jacobian for two rigid bodies + Gbar1 = blockdiag(sparse(I(3)),sparse(G(z[idx_z1[4:7]]))) + Gbar2 = blockdiag(sparse(I(3)),sparse(G(z[idx_z2[4:7]]))) + Gbar = Matrix(blockdiag(Gbar1,Gbar2)) +end +function Gbar(w,idx) + # attitude jacobian for idx.interactions rigid bodies + G1 = (blockdiag([blockdiag(sparse(I(3)),sparse(G(w[idx.z[i][4:7]]))) for i = 1:idx.N_bodies]...)) + Matrix(blockdiag(G1,sparse(I(2*length(idx.interactions))))) +end + +function update_se3(state, delta) + # update position additively, attitude multiplicatively + [ + state[SA[1,2,3]] + delta[SA[1,2,3]]; + L(state[SA[4,5,6,7]]) * ρ(delta[SA[4,5,6]]) + ] +end +function update_w(w,Δ,idx) + # update our variable w that has positions, attitudes, slacks and λ's + wnew = deepcopy(w) + for i = 1:idx.N_bodies + wnew[idx.z[i]] = update_se3(w[idx.z[i]], Δ[idx.Δz[i]]) + end + wnew[idx.s] += Δ[idx.Δs] + wnew[idx.λ] += Δ[idx.Δλ] + wnew +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 contact_kkt(w₋, w, w₊, P, inertias, masses, idx, κ) + # KKT conditions for our NCP + s = w₊[idx.s] + λ = w₊[idx.λ] + + # DEL stuff for each body + DELs = [single_DEL(w₋[idx.z[i]],w[idx.z[i]],w₊[idx.z[i]],inertias[i],masses[i],h) for i = 1:idx.N_bodies] + + # now we add the contact jacobian functions stuff at middle time step + update_objects!(P,w,idx) + for k = 1:idx.N_interactions + i,j = idx.interactions[k] + D_α = reshape(dc.proximity_gradient(P[i],P[j])[2], 1, 14) + E = h * τ_mod * (D_α*Gbar_2_body(w, idx.z[i], idx.z[j]))'*[λ[k]] + DELs[i] += E[1:6] + DELs[j] += E[7:12] + end + + # now get contact stuff for + time step + update_objects!(P,w₊,idx) + αs = contacts(P,idx) + + [ + vcat(DELs...); # DEL's + contact jacobians + s - αs; # slack (s) must equal contact αs + (s .* λ) .- κ; # complimentarity between s and λ + ] +end +function contact_kkt_no_α(w₋, w, w₊, P, inertias, masses, idx, κ) + # here is the same function as above, but without any contact stuff for + # the + time step, this allows us to forwarddiff this function, and add + # our DCOL contact jacobians seperately + + s = w₊[idx.s] + λ = w₊[idx.λ] + + # DEL stuff for each body + DELs = [single_DEL(w₋[idx.z[i]],w[idx.z[i]],w₊[idx.z[i]],inertias[i],masses[i],h) for i = 1:length(P)] + + # now we add the jacobian functions stuff at middle time step + update_objects!(P,w,idx) + for k = 1:idx.N_interactions + i,j = idx.interactions[k] + D_α = reshape(dc.proximity_gradient(P[i],P[j])[2], 1, 14) + E = h * τ_mod * (D_α*Gbar_2_body(w, idx.z[i], idx.z[j]))'*[λ[k]] + DELs[i] += E[1:6] + DELs[j] += E[7:12] + end + + # this is commented out (see above function) + # now get contact stuff for + time step + # update_objects!(P,w₊,idx) + # αs = contacts(P,idx) + + [ + vcat(DELs...); + s; #- αs; + (s .* λ) .- κ; + ] +end +function linesearch(x,dx) + # nonnegative orthant analytical linesearch (check cvxopt documentation) + α = min(0.99, minimum([dx[i]<0 ? -x[i]/dx[i] : Inf for i = 1:length(x)])) +end +function ncp_solve(w₋, w, P, inertias, masses, idx) + w₊ = copy(w) #+ .1*abs.(randn(length(z))) + w₊[idx.s] .+= 1 + w₊[idx.λ] .+= 1 + + @printf "iter |∇ₓL| |c| κ μ α αs αλ\n" + @printf "--------------------------------------------------------------------------\n" + + for main_iter = 1:30 + rhs1 = -contact_kkt(w₋, w, w₊, P, inertias, masses, idx, 0) + if norm(rhs1)<1e-6 + @info "success" + return w₊ + end + + # forward diff for contact KKT + D = FD.jacobian(_w -> contact_kkt_no_α(w₋, w, _w, P, inertias, masses, idx, 0.0),w₊) + + # add DCOL contact jacobians in for all our stuff + update_objects!(P,w₊,idx) + for k = 1:idx.N_interactions + i,j = idx.interactions[k] + (dc.proximity(P[i],P[j])[1] - 1) + D_α = reshape(dc.proximity_gradient(P[i],P[j])[2], 1, 14) + D[idx.α[k],idx.z[i]] = -D_α[1,idx.z[1]] + D[idx.α[k],idx.z[j]] = -D_α[1,idx.z[2]] + end + + # use G bar to handle quats, then factorize the matrix + F = factorize(D*Gbar(w₊,idx)) + + # affine step + Δa = F\rhs1 + αa = 0.99*min(linesearch(w₊[idx.s], Δa[idx.Δs]), linesearch(w₊[idx.λ], Δa[idx.Δλ])) + + # duality gap growth + μ = dot(w₊[idx.s], w₊[idx.λ]) + μa = dot(w₊[idx.s] + αa*Δa[idx.Δs], w₊[idx.λ] + αa*Δa[idx.Δλ]) + σ = min(0.99,max(0,μa/μ))^3 + κ = max(min(σ*μ,1),1e-8) + + # centering step + rhs2 = -contact_kkt(w₋, w, w₊, P, inertias, masses, idx, κ) + Δ = F\rhs2 + + # linesearch + α1 = linesearch(w₊[idx.s], Δ[idx.Δs]) + α2 = linesearch(w₊[idx.λ], Δ[idx.Δλ]) + α = 0.99*min(α1, α2) + + # update + w₊ = update_w(w₊,α*Δ,idx) + + @printf("%3d %9.2e %9.2e %9.2e %9.2e % 6.4f % 6.4f % 6.4f\n", + main_iter, norm(rhs1[1:(6*idx.N_bodies)]), norm(rhs1[(6*idx.N_bodies) .+ (1:idx.N_interactions)]), κ, μ, α, α1, α2) + + end + error("NCP solver failed") +end + + +# time step +const h = 0.05 + + +Random.seed!(1) + +width = 1 +height = 1.5 +A = SA[ + 1/(3/8*width) 0 1/(3/4*height) + 0 1/(3/8*width) 1/(3/4*height) + -1/(3/8*width) 0 1/(3/4*height) + 0 -1/(3/8*width) 1/(3/4*height) + 0 0 -1/(1/4*height) +] +b = SA[1;1;1;1;1.0] + +# build up all of the objects in your scene +P = [dc.Cylinder(0.5,3.0), dc.Sphere(1.0),dc.Polytope(A,b)] +P_floor, mass_floor, inertia_floor = dc.create_rect_prism(1.0,1.0,1.0;attitude = :quat) +push!(P,P_floor) + +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, gravity=0) + +for contact in mech.contacts + contact.impulses[1] = contact.impulses[2] = [0] +end + +# create the indexing named tuple +N_bodies = length(P) +@assert length(P) == N_bodies +idx = create_indexing(N_bodies) + +# choose masses and inertias for everything (this is done lazily here) +masses = [bodies[i].mass for i in eachindex(bodies)] +inertias = [bodies[i].inertia for i in eachindex(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] + +# put it all in a vector w (this is the full vector for the ncp solve) +w0 = vcat([[rs[i];qs[i]] for i = 1:N_bodies]..., zeros(2*idx.N_interactions)) + +# 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] + +# use initial velocities to get a second initial condition +r2s = [(rs[i] + h*vs[i]) for i = 1:N_bodies] +q2s = [Dojo.Lmat(Quaternion(qs[i]...))*Dojo.vector(Dojo.axis_angle_to_quaternion(h*ωs[i])) for i = 1:N_bodies] +w1 = vcat([[r2s[i];q2s[i]] for i = 1:N_bodies]..., zeros(2*idx.N_interactions)) + +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 + +w0 = vcat([[mech.bodies[i].state.x1;Dojo.vector(mech.bodies[i].state.q1)] for i = 1:N_bodies]..., zeros(2*idx.N_interactions)) +w1 = vcat([[mech.bodies[i].state.x2;Dojo.vector(mech.bodies[i].state.q2)] for i = 1:N_bodies]..., zeros(2*idx.N_interactions)) + + +# setup sim time +N = 80 +storage = Storage(N-1,N_bodies) +Dojo.save_to_storage!(mech, storage, 1) +W = [zeros(length(w0)) for i = 1:N] +W[1] = w0 +W[2] = w1 + +for i = 2:N-1 + println("------------------ITER NUMBER $i--------------------") + W[i+1] = ncp_solve(W[i-1], W[i], P, inertias, masses, idx) + for (j,body) in enumerate(mech.bodies) + body.state.x2 = W[i+1][idx.z[j][1:3]] + body.state.q2 = Quaternion(W[i+1][idx.z[j][4:7]]...) + end + Dojo.save_to_storage!(mech, storage, i) +end + +visualize(mech, storage; visualize_floor=false, framerate=Inf) + +# vis = mc.Visualizer() +# mc.open(vis) + +# for i = 1:N_bodies +# dc.build_primitive!(vis, P[i], Symbol("P"*string(i)); α = 1.0) +# end + +# mc.setprop!(vis["/Background"], "top_color", colorant"transparent") + +# anim = mc.Animation(floor(Int,1/h)) + +# for k = 1:length(W) +# mc.atframe(anim, k) do +# for i = 1:N_bodies +# sym = Symbol("P"*string(i)) +# mc.settransform!(vis[sym], mc.Translation(W[k][idx.z[i][1:3]]) ∘ mc.LinearMap(dc.dcm_from_q(W[k][idx.z[i][SA[4,5,6,7]]]))) +# end +# end +# end +# mc.setanimation!(vis, anim) diff --git a/src/Dojo.jl b/src/Dojo.jl index 3bb7469ff..546c33a2f 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -103,14 +103,14 @@ include(joinpath("joints", "impulses.jl")) include(joinpath("contacts", "constraints.jl")) include(joinpath("contacts", "cone.jl")) include(joinpath("contacts", "collisions", "collision.jl")) -include(joinpath("contacts", "collisions", "point_to_segment.jl")) -include(joinpath("contacts", "collisions", "point_to_box_v2.jl")) +# include(joinpath("contacts", "collisions", "point_to_segment.jl")) +# include(joinpath("contacts", "collisions", "point_to_box_v2.jl")) include(joinpath("contacts", "collisions", "sphere_halfspace.jl")) -include(joinpath("contacts", "collisions", "sphere_sphere.jl")) +# include(joinpath("contacts", "collisions", "sphere_sphere.jl")) include(joinpath("contacts", "collisions", "general_collision.jl")) -include(joinpath("contacts", "collisions", "sphere_capsule.jl")) -include(joinpath("contacts", "collisions", "sphere_box.jl")) -include(joinpath("contacts", "collisions", "string.jl")) +# include(joinpath("contacts", "collisions", "sphere_capsule.jl")) +# include(joinpath("contacts", "collisions", "sphere_box.jl")) +# include(joinpath("contacts", "collisions", "string.jl")) include(joinpath("contacts", "velocity.jl")) include(joinpath("contacts", "impact.jl")) include(joinpath("contacts", "linear.jl")) diff --git a/src/contacts/utilities.jl b/src/contacts/utilities.jl index 39a2fd42a..8828ba1f1 100644 --- a/src/contacts/utilities.jl +++ b/src/contacts/utilities.jl @@ -52,32 +52,3 @@ end function contact_location(mechanism::Mechanism) return [contact_location(mech, contact) for contact in mechanism.contacts] end - -#=""" - string_location(contact, x, q) - - location of the attach point of the string in world coordinates - - collision: StringCollision - x: body position - q: body orientation -"""=# -function string_location(collision::StringCollision, x::AbstractVector{T}, - q::Quaternion{T}; relative::Symbol=:parent) where T - origin = (relative == :parent) ? collision.origin_parent : collision.origin_child - return x + vector_rotate(origin, q) -end - -function string_location(collision::StringCollision, body::Body; relative::Symbol=:parent) - x = body.state.x2 - q = body.state.q2 - return string_location(collision, x, q, relative=relative) -end - -function string_location(mechanism::Mechanism, collision::StringCollision) - pbody = get_body(mechanism, contact.parent_id) - cbody = get_body(mechanism, contact.child_id) - x_parent = string_location(collision, pbody, relative=:parent) - x_child = string_location(collision, cbody, relative=:child) - return x_parent, x_child -end