Off-center contact

benchmark/example_benchmark.jl

@@ -10,7 +10,6 @@ files = [
     "doublependulum_disconnection"
     "doublependulum_urdf"
     "elliptic_joint"
-    "football"
     "fourbar_disconnection"
     "inverted_pyramid_plane"
     "joint_force"
@@ -34,7 +33,6 @@ controlled = [
     "joint_torque"
     "pendulum_forced"
     "nutation"
-    "football"
     "fourbar_disconnection"
     "slider_crank"
 ]

examples/dice.jl

@@ -10,6 +10,7 @@ width, depth = 0.5, 0.5
 box1 = Box(width, depth, length1, 1., color = RGBA(1., 1., 0.))
 b1 = Box(0.1, 0.1, .1, .1, color = RGBA(1., 0., 0.))
 
+# Corner vectors
 corners = [
     [[length1 / 2;length1 / 2;-length1 / 2]]
     [[length1 / 2;-length1 / 2;-length1 / 2]]
@@ -21,37 +22,31 @@ corners = [
     [[-length1 / 2;-length1 / 2;length1 / 2]]
 ]
 
+# Initial orientation
+ϕ1 = 0;
+q1 = UnitQuaternion(RotX(ϕ1))
+
 # Links
 origin = Origin{Float64}()
-
 link1 = Body(box1)
-links = [link1;[Body(b1) for i = 1:8]]
 
 # Constraints
-cf = 0.2
-ineqcs = [InequalityConstraint(Friction(links[i + 1], [0;0;1.0], cf)) for i = 1:8]
+impacts = [InequalityConstraint(Friction(link1,[0;0;1.0], 0.2; p = corners[i])) for i=1:8]
 
 joint0to1 = EqualityConstraint(Floating(origin, link1))
-eqcs = [joint0to1;[EqualityConstraint(Fixed(link1, links[i + 1]; p1=corners[i])) for i = 1:8]]
 
-shapes = [box1;b1]
+links = [link1]
+eqcs = [joint0to1]
+ineqcs = impacts
+shapes = [box1]
 
 
 mech = Mechanism(origin, links, eqcs, ineqcs, shapes = shapes)
-setPosition!(link1,x = [0.;-2;1.5])
-for i = 1:8
-    setPosition!(link1, links[i + 1], p1 = corners[i])
-end
 
+setPosition!(link1,x = [0.;-2;1.5])
 ωtemp = (rand(3) .- 0.5) * 100
-# ωtemp = [12.15437;5.08323;-39.13073]
-# ωtemp = [19.46637;-18.17827;-44.33827]
-# ωtemp = [-45.36396;23.93890;43.18141]
-setVelocity!(link1,v = [0;3;7.],ω = ωtemp)
-for i = 1:8
-    setVelocity!( link1, links[i + 1], p1 = corners[i])
-end
 
+setVelocity!(link1,v = [0;3;7.],ω = ωtemp)
 
-storage = simulate!(mech, 10., record = true)
+storage = simulate!(mech, 10, record = true)
 visualize(mech, storage, shapes)

src/bounds/bound.jl

@@ -4,15 +4,21 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, bound::Bound{T}) wh
     summary(io, bound)
 end
 
+### General functions
 @inline getT(bound::Bound{T}) where T = T
 
 
-@inline g(bound::Bound{T}) where T = zero(T)
+### Constraints and derivatives
+## Position level constraint wrappers
+g(contact::Bound, body::Body, Δt) = g(contact, body.state, Δt)
+
+
+# @inline g(bound::Bound{T}) where T = zero(T)
+
+# @inline ∂g∂pos(bound::Bound{T}) where T = szeros(T, 6)
+# @inline ∂g∂vel(bound::Bound{T}) where T = szeros(T, 6)
+# @inline schurf(bound::Bound{T}) where T = szeros(T, 6)
+# @inline schurD(bound::Bound{T}) where T = szeros(T, 6, 6)
 
-@inline ∂g∂pos(bound::Bound{T}) where T = szeros(T, 6)
-@inline ∂g∂vel(bound::Bound{T}) where T = szeros(T, 6)
-@inline schurf(bound::Bound{T}) where T = szeros(T, 6)
-@inline schurD(bound::Bound{T}) where T = szeros(T, 6, 6)
 
-@inline additionalforce(bound::Bound{T}) where T = szeros(T, 6)
 

src/bounds/contact.jl

@@ -1,14 +1,77 @@
 abstract type Contact{T} <: Bound{T} end
 
-@inline g(contact::Contact, body::Body, Δt) = g(contact, body.state, Δt)
+### Constraints and derivatives
+## Discrete-time position wrappers (for dynamics)
+@inline g(contact::Contact, state::State, Δt) = g(contact, posargsnext(state, Δt)...)
 
-@inline ∂g∂pos(contact::Contact, body::Body) = ∂g∂pos(contact, body.state)
-@inline ∂g∂vel(contact::Contact, body::Body, Δt) = ∂g∂vel(contact, body.state, Δt)
+## Position level constraints (for dynamics)
+# @inline g(contact::Contact{T}) where {T} = szeros(T,6)
+@inline g(contact::Contact, x::AbstractVector, q::UnitQuaternion) = contact.ainv3 * (x + vrotate(contact.p,q) - contact.offset)
 
+
+## Derivatives NOT accounting for quaternion specialness
+@inline function ∂g∂pos(contact::Contact, x::AbstractVector, q::UnitQuaternion)
+    p = contact.p
+    X = contact.ainv3
+    Q = contact.ainv3 * (VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p)))
+    return X, Q
+end
+
+## Discrete-time position derivatives (for dynamics)
+# Wrappers 1
+@inline function ∂g∂ʳpos(contact::Contact, body::Body)
+    return ∂g∂ʳpos(contact, body.state)
+end
+
+#Wrappers 2
+@inline ∂g∂ʳpos(contact::Contact{T}) where T = szeros(T, 6)
+∂g∂ʳpos(contact::Contact, state::State) = ∂g∂ʳpos(contact, posargsk(state)...)
+
+#Derivatives accounting for quaternion specialness
+@inline function ∂g∂ʳpos(contact::Contact, x::AbstractVector, q::UnitQuaternion)
+    X, Q = ∂g∂pos(contact, x, q)
+    Q = Q * LVᵀmat(q)
+
+    return [X Q]
+end
+
+## Discrete-time velocity derivatives (for dynamics)
+# Wrappers 1
+@inline function ∂g∂ʳvel(contact::Contact, body::Body, Δt)
+    return ∂g∂ʳvel(contact, body.state, Δt)
+end
+
+#Wrappers 2
+@inline ∂g∂ʳvel(contact::Contact{T}) where {T} = szeros(T, 6)
+∂g∂ʳvel(contact::Contact, state::State, Δt) = ∂g∂ʳvel(contact, posargsnext(state, Δt)..., fullargssol(state)..., Δt)
+
+#Derivatives accounting for quaternion specialness
+@inline function ∂g∂ʳvel(contact::Contact,x2::AbstractVector, q2::UnitQuaternion, x1::AbstractVector,v1::AbstractVector, q1::UnitQuaternion,w1::AbstractVector, Δt)
+    X, Q = ∂g∂pos(contact, x2, q2)
+    V = X * Δt
+    Ω = Q * Lmat(q1) * derivωbar(w1, Δt)
+    return [V Ω]
+end
+
+##Schurf und SchurD
+#Schurf
 @inline function schurf(ineqc, contact::Contact, i, body::Body, μ, Δt)
-    return schurf(contact, body.state, ineqc.γsol[2][i], ineqc.ssol[2][i], μ, Δt)
+    return schurf(contact, body.state, ineqc.γsol[2][i], ineqc.ssol[2][i], μ, Δt)[SA[1; 2; 3; 4; 5; 6]]
+end
+@inline function schurf(contact::Contact, state::State, γ, s, μ, Δt)
+    φ = g(contact, posargsnext(state, Δt)...)
+    return ∂g∂ʳpos(contact, state)' * (γ / s * φ - μ / s)
 end
 
+#SchurD
 @inline function schurD(ineqc, contact::Contact, i, body::Body, Δt)
     return schurD(contact, body.state, ineqc.γsol[2][i], ineqc.ssol[2][i], Δt)
-end
+end
+@inline function schurD(contact::Contact,state::State, γ, s, Δt)
+    return ∂g∂ʳpos(contact, state)' * γ/s * ∂g∂ʳvel(contact, state, Δt)
+end
+
+
+## Additional force for friction
+@inline additionalforce(bound::Contact, state::State) = additionalforce(bound, posargsk(state)...)
+@inline additionalforce(bound::Contact, body::Body) = additionalforce(bound, body.state)

src/bounds/friction.jl

@@ -1,39 +1,83 @@
 mutable struct Friction{T} <: Contact{T}
-    Nx::Adjoint{T,SVector{6,T}}
-    D::SMatrix{2,6,T,12}
+    ainv3::Adjoint{T,SVector{3,T}}
+    p::SVector{3,T}
+    D::SMatrix{2,3,T,6}
     cf::T
-    offset::SVector{6,T}
+    offset::SVector{3,T}
     b::SVector{2,T}
 
 
-    function Friction(body::Body{T}, normal::AbstractVector, cf::Real; offset::AbstractVector = zeros(3)) where T
+    function Friction(body::Body{T}, normal::AbstractVector, cf::Real; p::AbstractVector = zeros(3), offset::AbstractVector = zeros(3)) where T
         @assert cf>0
 
         # Derived from plane equation a*v1 + b*v2 + distance*v3 = p - offset
         V1, V2, V3 = orthogonalcols(normal) # gives two plane vectors and the original normal axis
         A = [V1 V2 V3]
         Ainv = inv(A)
-        ainv3 = Ainv[3,SA[1; 2; 3]]
-        Nx = [ainv3;0.0;0.0;0.0]'
-        D = [[V1 V2];szeros(3,2)]'
-        offset = [offset;0.0;0.0;0.0]
+        ainv3 = Ainv[3,SA[1; 2; 3]]'
+        D = [V1 V2]'
 
-        new{T}(Nx, D, cf, offset, zeros(2)), body.id
+        new{T}(ainv3, p, D, cf, offset, zeros(2)), body.id
     end
 end
 
 function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::Friction{T}) where {T}
     summary(io, constraint)
     println(io,"")
-    println(io, " Nx:     "*string(constraint.Nx))
+    println(io, " ainv3:  "*string(constraint.ainv3))
     println(io, " D:      "*string(constraint.D))
     println(io, " cf:     "*string(constraint.cf))
     println(io, " offset: "*string(constraint.offset))
 end
 
+@inline function calcb0(friction::Friction, m, Δt, x::AbstractVector, q::UnitQuaternion, vc, ωc, dyn)
+    p = friction.p
+    np2 = norm(p)^2
+    vp = vc + vrotate(cross(ωc,p),q)
+    if np2 > 0
+        return friction.D * (I*m*(-vp)/Δt + dyn[SA[1;2;3]] + vrotate(cross(dyn[SA[4;5;6]]/2,p),q)/np2)
+    else
+        return friction.D * (I*m*(-vp)/Δt + dyn[SA[1;2;3]])
+    end
+end
+
+@inline function frictionmap(friction::Friction, x::AbstractVector, q::UnitQuaternion)
+    Fp = friction.D' * friction.b
+    Fc = Fp
+    τc = torqueFromForce(vrotate(Fp,inv(q)), friction.p) # in local coordinates
+    return [Fc;2*τc]
+end
+
+@inline additionalforce(friction::Friction, x::AbstractVector, q::UnitQuaternion) = frictionmap(friction, x, q)
 
-@inline additionalforce(friction::Friction) = friction.D'*friction.b
 @inline function calcFrictionForce!(mechanism, ineqc, friction::Friction, i, body::Body)
     calcFrictionForce!(mechanism, friction, body, ineqc.γsol[2][i])
     return
+end
+
+##Friction
+# TODO this might be wrong, will be done differently in future anyways
+@inline function calcFrictionForce!(mechanism::Mechanism{T}, friction::Contact, body::Body, γ) where T
+    cf = friction.cf
+    D = friction.D
+    state = body.state
+
+    d = state.d
+    v = state.vsol[2]
+    ω = state.ωsol[2]
+    state.vsol[2] = state.vc
+    state.ωsol[2] = state.ωc
+    dyn = dynamics(mechanism, body) + frictionmap(friction, posargsk(body.state)...)
+    state.vsol[2] = v
+    state.ωsol[2] = ω
+    state.d = d
+
+    b0 = calcb0(friction, body.m, mechanism.Δt, posargsk(body.state)..., state.vc, state.ωc, dyn)
+
+    if norm(b0) > cf*γ
+        friction.b = b0/norm(b0)*cf*γ
+    else
+        friction.b = b0
+    end    
+    return
 end

src/bounds/impact.jl

@@ -1,24 +1,28 @@
-mutable struct Impact{T} <: Contact{T}
-    Nx::Adjoint{T,SVector{6,T}}
-    offset::SVector{6,T}
 
+mutable struct Impact{T} <: Contact{T}
+    ainv3::Adjoint{T,SVector{3,T}}
+    p::SVector{3,T}
+    offset::SVector{3,T}
+
 
-    function Impact(body::Body{T}, normal::AbstractVector; offset::AbstractVector = zeros(3)) where T
+    function Impact(body::Body{T}, normal::AbstractVector; p::AbstractVector = zeros(3),  offset::AbstractVector = zeros(3)) where T
         # Derived from plane equation a*v1 + b*v2 + distance*v3 = p - offset
         V1, V2, V3 = orthogonalcols(normal) # gives two plane vectors and the original normal axis
         A = [V1 V2 V3]
         Ainv = inv(A)
-        ainv3 = Ainv[3,SA[1; 2; 3]]
-        Nx = [ainv3;0.0;0.0;0.0]'
-        offset = [offset;0.0;0.0;0.0]
+        ainv3 = Ainv[3,SA[1; 2; 3]]'
 
-        new{T}(Nx, offset), body.id
+        new{T}(ainv3, p, offset), body.id
     end
 end
 
 function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::Impact{T}) where {T}
     summary(io, constraint)
     println(io,"")
-    println(io, " Nx:     "*string(constraint.Nx))
+    println(io, " ainv3:  "*string(constraint.ainv3))
     println(io, " offset: "*string(constraint.offset))
-end
+end
+
+
+## Additional force for friction default
+@inline additionalforce(bound::Impact{T}, x::AbstractVector, q::UnitQuaternion) where T = szeros(T, 6)

src/discretization/ImplicitTrapezoid.jl

@@ -1,5 +1,3 @@
 include(joinpath("implicit_trapezoid", "integrator.jl"))
 include(joinpath("implicit_trapezoid", "body.jl"))
-# include(joinpath("implicit_trapezoid", "joint.jl"))
-include(joinpath("implicit_trapezoid", "bound.jl"))
 include(joinpath("implicit_trapezoid", "test.jl"))

src/discretization/SymplecticEuler.jl

@@ -1,5 +1,3 @@
 include(joinpath("symplectic_euler", "integrator.jl"))
 include(joinpath("symplectic_euler", "body.jl"))
-# include(joinpath("symplectic_euler", "joint.jl"))
-include(joinpath("symplectic_euler", "bound.jl"))
 include(joinpath("symplectic_euler", "test.jl"))