From c10f1b31ef11b47209cda66d084f6be2b1cd5c87 Mon Sep 17 00:00:00 2001 From: JanaJaneva Date: Sat, 12 Dec 2020 14:41:43 +0100 Subject: [PATCH 1/7] my changes --- examples/dice.jl | 41 +++++---------- examples/pendulum.jl | 2 +- src/bounds/contact.jl | 52 ++++++++++++++++++-- src/bounds/friction.jl | 4 +- src/bounds/impact.jl | 12 +++-- src/discretization/symplectic_euler/bound.jl | 34 ++++++------- src/joints/abstract_joint.jl | 14 ------ src/main_components/inequalityconstraint.jl | 10 ++-- src/main_components/mechanism_functions.jl | 2 +- src/solver/newton.jl | 2 +- src/solver/solverfunctions.jl | 10 ++-- src/util/quaternion.jl | 3 ++ 12 files changed, 102 insertions(+), 84 deletions(-) diff --git a/examples/dice.jl b/examples/dice.jl index 2732361b..bb5155ca 100644 --- a/examples/dice.jl +++ b/examples/dice.jl @@ -10,48 +10,33 @@ 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.)) -corners = [ - [[length1 / 2;length1 / 2;-length1 / 2]] - [[length1 / 2;-length1 / 2;-length1 / 2]] - [[-length1 / 2;length1 / 2;-length1 / 2]] - [[-length1 / 2;-length1 / 2;-length1 / 2]] - [[length1 / 2;length1 / 2;length1 / 2]] - [[length1 / 2;-length1 / 2;length1 / 2]] - [[-length1 / 2;length1 / 2;length1 / 2]] - [[-length1 / 2;-length1 / 2;length1 / 2]] -] +#Corner Vector v +v = [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] +impact = InequalityConstraint(Impact(link1,[0;0;1.0]; p = v)) joint0to1 = EqualityConstraint(Floating(origin, link1)) -eqcs = [joint0to1;[EqualityConstraint(Fixed(link1, links[i + 1]; p1=corners[i])) for i = 1:8]] -shapes = [box1;b1] +eqcs = [joint0to1] +ineqcs = [impact] +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 +mech = Mechanism(origin, [link1], eqcs, ineqcs, shapes = shapes) +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) visualize(mech, storage, shapes) diff --git a/examples/pendulum.jl b/examples/pendulum.jl index a7d7cb6b..5d4d1f31 100644 --- a/examples/pendulum.jl +++ b/examples/pendulum.jl @@ -9,7 +9,7 @@ length1 = 1.0 width, depth = 0.1, 0.1 box = Box(width, depth, length1, length1) -p2 = [0.0;0.0;length1 / 2] # joint connection point +p2 = [0.0;0.0;length1/2] # joint connection point # Initial orientation ϕ1 = π / 2 diff --git a/src/bounds/contact.jl b/src/bounds/contact.jl index f5bcf2db..ad8b30e1 100644 --- a/src/bounds/contact.jl +++ b/src/bounds/contact.jl @@ -1,14 +1,56 @@ abstract type Contact{T} <: Bound{T} end -@inline g(contact::Contact, body::Body, Δt) = g(contact, body.state, Δt) +###Constraints and Derivatives +##Position level constraint wrappers +g(contact::Contact, body::Body, Δt) = g(contact, body.state, Δt) +@inline g(contact::Contact{T}) where {T} = szeros(T,6) -@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) +##Descrete 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 + +##Descrete Time Velocity Derivatives +#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 sccounting 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 + + +#ShurF @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)[1:6] end + +#ShurD @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 \ No newline at end of file +end + diff --git a/src/bounds/friction.jl b/src/bounds/friction.jl index 51bf427d..56ba00c6 100644 --- a/src/bounds/friction.jl +++ b/src/bounds/friction.jl @@ -1,5 +1,5 @@ mutable struct Friction{T} <: Contact{T} - Nx::Adjoint{T,SVector{6,T}} + Nx::Adjoint{T,SVector{3,T}} D::SMatrix{2,6,T,12} cf::T offset::SVector{6,T} @@ -14,7 +14,7 @@ mutable struct Friction{T} <: Contact{T} A = [V1 V2 V3] Ainv = inv(A) ainv3 = Ainv[3,SA[1; 2; 3]] - Nx = [ainv3;0.0;0.0;0.0]' + Nx = ainv3' D = [[V1 V2];szeros(3,2)]' offset = [offset;0.0;0.0;0.0] diff --git a/src/bounds/impact.jl b/src/bounds/impact.jl index 97be4284..fb16b419 100644 --- a/src/bounds/impact.jl +++ b/src/bounds/impact.jl @@ -1,18 +1,20 @@ + mutable struct Impact{T} <: Contact{T} - Nx::Adjoint{T,SVector{6,T}} + Nx::Adjoint{T,SVector{3,T}} + p::SVector{3,T} offset::SVector{6,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]' + Nx = ainv3' offset = [offset;0.0;0.0;0.0] - new{T}(Nx, offset), body.id + new{T}(Nx, p, offset), body.id end end diff --git a/src/discretization/symplectic_euler/bound.jl b/src/discretization/symplectic_euler/bound.jl index 19c8f6c2..b48acf29 100644 --- a/src/discretization/symplectic_euler/bound.jl +++ b/src/discretization/symplectic_euler/bound.jl @@ -1,27 +1,27 @@ -@inline g(contact::Contact, x) = contact.Nx[SA[1; 2; 3]]' * (x - contact.offset[SA[1; 2; 3]]) -@inline g(contact::Contact, state::State, Δt) = g(contact, getx3(state, Δt)) +@inline g(contact::Contact, x, q) = contact.Nx[SA[1; 2; 3]]' * (x + vrotate(contact.p,q) - contact.offset[SA[1; 2; 3]]) #c entfernung mit orientierung +@inline g(contact::Contact, state::State, Δt) = g(contact, getx3(state, Δt), getq3(state, Δt)) + +##Descrete Time Position Derivatives +@inline function ∂g∂pos(contact::Contact, x::AbstractVector, q::UnitQuaternion) + X = contact.Nx #∂g∂x + Q = contact.Nx * ((VLmat(q) * Lmat(UnitQuaternion(contact.p)) * Tmat()) + (VRᵀmat(q) * Rmat(UnitQuaternion(contact.p)))) #∂g∂q + return X, Q #[∂g∂x ∂g∂q] +end -@inline ∂g∂pos(contact::Contact) = contact.Nx -@inline ∂g∂pos(contact::Contact, state::State) = ∂g∂pos(contact) -@inline ∂g∂vel(contact::Contact, Δt) = contact.Nx * Δt -@inline ∂g∂vel(contact::Contact, state::State, Δt) = ∂g∂vel(contact, Δt) -@inline schurf(contact::Contact, φ, γ, s, μ) = ∂g∂pos(contact)' * (γ / s * φ - μ / s) +##Shurf und ShurD +#Shurf @inline function schurf(contact::Contact, state::State, γ, s, μ, Δt) - φ = g(contact, getx3(state, Δt)) - return schurf(contact, φ, γ, s, μ) + φ = g(contact, getx3(state, Δt), getq3(state, Δt)) + return ∂g∂ʳpos(contact, state)' * (γ / s * φ - μ / s) end - -@inline function schurD(contact::Contact, γ, s, Δt) - Nx = ∂g∂pos(contact) - Nv = ∂g∂vel(contact, Δt) - - return Nx' * γ / s * Nv +#ShurD +@inline function schurD(contact::Contact,state::State, γ, s, Δt) + return ∂g∂ʳpos(contact, state)' * γ/s * ∂g∂ʳvel(contact, state, Δt) end -@inline schurD(contact::Contact, state::State, γ, s, Δt) = schurD(contact, γ, s, Δt) - +#Friction @inline function calcFrictionForce!(mechanism::Mechanism{T}, friction::Contact, body::Body, γ) where T cf = friction.cf D = friction.D diff --git a/src/joints/abstract_joint.jl b/src/joints/abstract_joint.jl index 3cc94541..0829e202 100644 --- a/src/joints/abstract_joint.jl +++ b/src/joints/abstract_joint.jl @@ -173,17 +173,3 @@ end return end - - - - - - - - - - - - - - diff --git a/src/main_components/inequalityconstraint.jl b/src/main_components/inequalityconstraint.jl index 823fce0a..90b5cadf 100644 --- a/src/main_components/inequalityconstraint.jl +++ b/src/main_components/inequalityconstraint.jl @@ -57,7 +57,7 @@ end @inline function NtγTof!(mechanism, body::Body, ineqc::InequalityConstraint{T,N}) where {T,N} if isactive(ineqc) state = body.state - state.d -= ∂g∂pos(mechanism, ineqc, body)' * ineqc.γsol[2] + state.d -= ∂g∂ʳpos(mechanism, ineqc, body)' * ineqc.γsol[2] for i=1:N state.d -= additionalforce(ineqc.constraints[i]) end @@ -108,13 +108,13 @@ function schurD(ineqc::InequalityConstraint{T,N}, body, Δt) where {T,N} return val end -@generated function ∂g∂pos(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} - vec = [:(∂g∂pos(ineqc.constraints[$i])) for i = 1:N] +@generated function ∂g∂ʳpos(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} + vec = [:(∂g∂ʳpos(ineqc.constraints[$i], body)) for i = 1:N] return :(vcat($(vec...))) end -@generated function ∂g∂vel(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} - vec = [:(∂g∂vel(ineqc.constraints[$i], mechanism.Δt)) for i = 1:N] +@generated function ∂g∂ʳvel(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} + vec = [:(∂g∂ʳvel(ineqc.constraints[$i], body, mechanism.Δt)) for i = 1:N] return :(vcat($(vec...))) end diff --git a/src/main_components/mechanism_functions.jl b/src/main_components/mechanism_functions.jl index 40d162cc..a315b193 100644 --- a/src/main_components/mechanism_functions.jl +++ b/src/main_components/mechanism_functions.jl @@ -235,7 +235,7 @@ function disassemble(mechanism::Mechanism{T}; shapes::Vector{<:Shape} = Shape{T} # Set origin to next id oldoid = origin.id - origin.id = getGlobalID() + origin.id = getGlobalID()b for eqc in eqconstraints if eqc.parentid === nothing eqc.parentid = origin.id diff --git a/src/solver/newton.jl b/src/solver/newton.jl index 6d0bab23..c8991516 100644 --- a/src/solver/newton.jl +++ b/src/solver/newton.jl @@ -48,7 +48,7 @@ function newton!(mechanism::Mechanism{T,Nn,Nb,Ne,Ni}; meritf0 = meritf(mechanism) for n = Base.OneTo(newtonIter) - setentries!(mechanism) + setentries!(mechanism) #wo mein Matrix steht da, da ist auch extendDanddeltas (rechte Seite) factor!(graph, ldu) solve!(mechanism) # x̂1 for each body and constraint diff --git a/src/solver/solverfunctions.jl b/src/solver/solverfunctions.jl index 90e5aff5..a7d20f5d 100644 --- a/src/solver/solverfunctions.jl +++ b/src/solver/solverfunctions.jl @@ -5,7 +5,7 @@ end @inline function extendDandΔs!(mechanism::Mechanism, diagonal::DiagonalEntry, body::Body, ineqc::InequalityConstraint) - diagonal.D += schurD(ineqc, body, mechanism.Δt) # + SMatrix{6,6,Float64,36}(1e-5*I) + diagonal.D += schurD(ineqc, body, mechanism.Δt) # + SMatrix{6,6,Float64,36}(1e-5*I) #BODY UM DIE POSITION ZU HABEN diagonal.Δs += schurf(mechanism, ineqc, body) return end @@ -118,9 +118,9 @@ function setentries!(mechanism::Mechanism) diagonal = getentry(ldu, id) setDandΔs!(mechanism, diagonal, body) - for childid in ineqchildren(graph, id) + for childid in ineqchildren(graph, id) ineqc = getineqconstraint(mechanism, childid) - calcFrictionForce!(mechanism, ineqc) + calcFrictionForce!(mechanism, ineqc) #OB ES FRICTION ODER IMPACT IST ! extendDandΔs!(mechanism, diagonal, body, ineqc) end end @@ -218,8 +218,8 @@ function eliminatedsolve!(mechanism::Mechanism, ineqentry::InequalityEntry, diag φ = g(mechanism, ineqc) - Nx = ∂g∂pos(mechanism, ineqc, body) - Nv = ∂g∂vel(mechanism, ineqc, body) + Nx = ∂g∂ʳpos(mechanism, ineqc, body) + Nv = ∂g∂ʳvel(mechanism, ineqc, body) γ1 = ineqc.γsol[2] s1 = ineqc.ssol[2] diff --git a/src/util/quaternion.jl b/src/util/quaternion.jl index 70d4f627..11104441 100644 --- a/src/util/quaternion.jl +++ b/src/util/quaternion.jl @@ -11,6 +11,8 @@ vrotate(v::StaticVector,q::UnitQuaternion) = q*v rotation_vector(q::UnitQuaternion) = rotation_angle(q) * rotation_axis(q) + + Lmat(q) = lmult(q) Lᵀmat(q) = lmult(q)' Rmat(q) = rmult(q) @@ -22,6 +24,7 @@ Vmat(::Type{T}=Float64) where T = vmat(T) Vᵀmat(::Type{T}=Float64) where T = hmat(T) Vmat(q::UnitQuaternion) = imag(q) + function VLmat(q::UnitQuaternion) SA[ q.x q.w -q.z q.y; From c085de43ec78301d7bd187f977ea285e5ce0d861 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Sun, 13 Dec 2020 21:45:10 +0100 Subject: [PATCH 2/7] Initial cleanup --- examples/pendulum.jl | 2 +- src/joints/abstract_joint.jl | 1 - src/main_components/mechanism_functions.jl | 2 +- src/solver/newton.jl | 2 +- src/solver/solverfunctions.jl | 6 +++--- src/util/quaternion.jl | 3 --- 6 files changed, 6 insertions(+), 10 deletions(-) diff --git a/examples/pendulum.jl b/examples/pendulum.jl index 5d4d1f31..a7d7cb6b 100644 --- a/examples/pendulum.jl +++ b/examples/pendulum.jl @@ -9,7 +9,7 @@ length1 = 1.0 width, depth = 0.1, 0.1 box = Box(width, depth, length1, length1) -p2 = [0.0;0.0;length1/2] # joint connection point +p2 = [0.0;0.0;length1 / 2] # joint connection point # Initial orientation ϕ1 = π / 2 diff --git a/src/joints/abstract_joint.jl b/src/joints/abstract_joint.jl index 0829e202..7e3f3e0d 100644 --- a/src/joints/abstract_joint.jl +++ b/src/joints/abstract_joint.jl @@ -172,4 +172,3 @@ end applyFτ!(joint, body2.state, clear) return end - diff --git a/src/main_components/mechanism_functions.jl b/src/main_components/mechanism_functions.jl index a315b193..40d162cc 100644 --- a/src/main_components/mechanism_functions.jl +++ b/src/main_components/mechanism_functions.jl @@ -235,7 +235,7 @@ function disassemble(mechanism::Mechanism{T}; shapes::Vector{<:Shape} = Shape{T} # Set origin to next id oldoid = origin.id - origin.id = getGlobalID()b + origin.id = getGlobalID() for eqc in eqconstraints if eqc.parentid === nothing eqc.parentid = origin.id diff --git a/src/solver/newton.jl b/src/solver/newton.jl index c8991516..6d0bab23 100644 --- a/src/solver/newton.jl +++ b/src/solver/newton.jl @@ -48,7 +48,7 @@ function newton!(mechanism::Mechanism{T,Nn,Nb,Ne,Ni}; meritf0 = meritf(mechanism) for n = Base.OneTo(newtonIter) - setentries!(mechanism) #wo mein Matrix steht da, da ist auch extendDanddeltas (rechte Seite) + setentries!(mechanism) factor!(graph, ldu) solve!(mechanism) # x̂1 for each body and constraint diff --git a/src/solver/solverfunctions.jl b/src/solver/solverfunctions.jl index a7d20f5d..05940798 100644 --- a/src/solver/solverfunctions.jl +++ b/src/solver/solverfunctions.jl @@ -5,7 +5,7 @@ end @inline function extendDandΔs!(mechanism::Mechanism, diagonal::DiagonalEntry, body::Body, ineqc::InequalityConstraint) - diagonal.D += schurD(ineqc, body, mechanism.Δt) # + SMatrix{6,6,Float64,36}(1e-5*I) #BODY UM DIE POSITION ZU HABEN + diagonal.D += schurD(ineqc, body, mechanism.Δt) # + SMatrix{6,6,Float64,36}(1e-5*I) diagonal.Δs += schurf(mechanism, ineqc, body) return end @@ -118,9 +118,9 @@ function setentries!(mechanism::Mechanism) diagonal = getentry(ldu, id) setDandΔs!(mechanism, diagonal, body) - for childid in ineqchildren(graph, id) + for childid in ineqchildren(graph, id) ineqc = getineqconstraint(mechanism, childid) - calcFrictionForce!(mechanism, ineqc) #OB ES FRICTION ODER IMPACT IST ! + calcFrictionForce!(mechanism, ineqc) extendDandΔs!(mechanism, diagonal, body, ineqc) end end diff --git a/src/util/quaternion.jl b/src/util/quaternion.jl index 11104441..70d4f627 100644 --- a/src/util/quaternion.jl +++ b/src/util/quaternion.jl @@ -11,8 +11,6 @@ vrotate(v::StaticVector,q::UnitQuaternion) = q*v rotation_vector(q::UnitQuaternion) = rotation_angle(q) * rotation_axis(q) - - Lmat(q) = lmult(q) Lᵀmat(q) = lmult(q)' Rmat(q) = rmult(q) @@ -24,7 +22,6 @@ Vmat(::Type{T}=Float64) where T = vmat(T) Vᵀmat(::Type{T}=Float64) where T = hmat(T) Vmat(q::UnitQuaternion) = imag(q) - function VLmat(q::UnitQuaternion) SA[ q.x q.w -q.z q.y; From 01969fb5dc49c2d65eb13751e6f897d1155c9420 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Sun, 13 Dec 2020 22:04:54 +0100 Subject: [PATCH 3/7] Second cleanup --- examples/dice.jl | 15 ++++++++++--- src/bounds/contact.jl | 10 ++++----- src/bounds/friction.jl | 8 +++---- src/bounds/impact.jl | 3 +-- src/discretization/symplectic_euler/bound.jl | 22 +++++++++++--------- src/main_components/inequalityconstraint.jl | 10 ++++----- src/solver/solverfunctions.jl | 8 +++---- 7 files changed, 43 insertions(+), 33 deletions(-) diff --git a/examples/dice.jl b/examples/dice.jl index bb5155ca..d3e25f01 100644 --- a/examples/dice.jl +++ b/examples/dice.jl @@ -11,7 +11,16 @@ 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 Vector v -v = [length1 / 2; length1 / 2; -length1 / 2] +corners = [ + [[length1 / 2;length1 / 2;-length1 / 2]] + [[length1 / 2;-length1 / 2;-length1 / 2]] + [[-length1 / 2;length1 / 2;-length1 / 2]] + [[-length1 / 2;-length1 / 2;-length1 / 2]] + [[length1 / 2;length1 / 2;length1 / 2]] + [[length1 / 2;-length1 / 2;length1 / 2]] + [[-length1 / 2;length1 / 2;length1 / 2]] + [[-length1 / 2;-length1 / 2;length1 / 2]] +] # Initial orientation ϕ1 = 0; @@ -22,12 +31,12 @@ origin = Origin{Float64}() link1 = Body(box1) # Constraints -impact = InequalityConstraint(Impact(link1,[0;0;1.0]; p = v)) +impacts = [InequalityConstraint(Impact(link1,[0;0;1.0]; p = corners[i])) for i=1:8] joint0to1 = EqualityConstraint(Floating(origin, link1)) eqcs = [joint0to1] -ineqcs = [impact] +ineqcs = impacts shapes = [box1] diff --git a/src/bounds/contact.jl b/src/bounds/contact.jl index ad8b30e1..1cabdec5 100644 --- a/src/bounds/contact.jl +++ b/src/bounds/contact.jl @@ -6,7 +6,7 @@ g(contact::Contact, body::Body, Δt) = g(contact, body.state, Δt) @inline g(contact::Contact{T}) where {T} = szeros(T,6) -##Descrete Time Position Derivatives (for dynamics) +##Discrete Time Position Derivatives (for dynamics) #Wrappers 1 @inline function ∂g∂ʳpos(contact::Contact, body::Body) return ∂g∂ʳpos(contact, body.state) @@ -24,7 +24,7 @@ end return [X Q] end -##Descrete Time Velocity Derivatives +##Discrete Time Velocity Derivatives #Wrappers 1 @inline function ∂g∂ʳvel(contact::Contact, body::Body, Δt) return ∂g∂ʳvel(contact, body.state, Δt) @@ -34,7 +34,7 @@ end @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 sccounting for quaternion specialness +#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 @@ -43,13 +43,13 @@ end end -#ShurF +#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)[1:6] end -#ShurD +#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 diff --git a/src/bounds/friction.jl b/src/bounds/friction.jl index 56ba00c6..2ab7ae4c 100644 --- a/src/bounds/friction.jl +++ b/src/bounds/friction.jl @@ -1,12 +1,13 @@ mutable struct Friction{T} <: Contact{T} Nx::Adjoint{T,SVector{3,T}} + p::SVector{3,T} D::SMatrix{2,6,T,12} 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 @@ -16,9 +17,8 @@ mutable struct Friction{T} <: Contact{T} ainv3 = Ainv[3,SA[1; 2; 3]] Nx = ainv3' D = [[V1 V2];szeros(3,2)]' - offset = [offset;0.0;0.0;0.0] - new{T}(Nx, D, cf, offset, zeros(2)), body.id + new{T}(Nx, p, D, cf, offset, zeros(2)), body.id end end diff --git a/src/bounds/impact.jl b/src/bounds/impact.jl index fb16b419..0c8364b1 100644 --- a/src/bounds/impact.jl +++ b/src/bounds/impact.jl @@ -2,7 +2,7 @@ mutable struct Impact{T} <: Contact{T} Nx::Adjoint{T,SVector{3,T}} p::SVector{3,T} - offset::SVector{6,T} + offset::SVector{3,T} function Impact(body::Body{T}, normal::AbstractVector; p::AbstractVector = zeros(3), offset::AbstractVector = zeros(3)) where T @@ -12,7 +12,6 @@ mutable struct Impact{T} <: Contact{T} Ainv = inv(A) ainv3 = Ainv[3,SA[1; 2; 3]] Nx = ainv3' - offset = [offset;0.0;0.0;0.0] new{T}(Nx, p, offset), body.id end diff --git a/src/discretization/symplectic_euler/bound.jl b/src/discretization/symplectic_euler/bound.jl index b48acf29..26d49e1c 100644 --- a/src/discretization/symplectic_euler/bound.jl +++ b/src/discretization/symplectic_euler/bound.jl @@ -1,27 +1,29 @@ -@inline g(contact::Contact, x, q) = contact.Nx[SA[1; 2; 3]]' * (x + vrotate(contact.p,q) - contact.offset[SA[1; 2; 3]]) #c entfernung mit orientierung +@inline g(contact::Contact, x, q) = contact.Nx * (x + vrotate(contact.p,q) - contact.offset) @inline g(contact::Contact, state::State, Δt) = g(contact, getx3(state, Δt), getq3(state, Δt)) -##Descrete Time Position Derivatives +##Discrete Time Position Derivatives @inline function ∂g∂pos(contact::Contact, x::AbstractVector, q::UnitQuaternion) - X = contact.Nx #∂g∂x - Q = contact.Nx * ((VLmat(q) * Lmat(UnitQuaternion(contact.p)) * Tmat()) + (VRᵀmat(q) * Rmat(UnitQuaternion(contact.p)))) #∂g∂q - return X, Q #[∂g∂x ∂g∂q] + p = contact.p + X = contact.Nx + Q = contact.Nx * (VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p))) + return X, Q end - -##Shurf und ShurD -#Shurf +##Schurf und SchurD +#Schurf @inline function schurf(contact::Contact, state::State, γ, s, μ, Δt) φ = g(contact, getx3(state, Δt), getq3(state, Δt)) return ∂g∂ʳpos(contact, state)' * (γ / s * φ - μ / s) end -#ShurD + +#SchurD @inline function schurD(contact::Contact,state::State, γ, s, Δt) return ∂g∂ʳpos(contact, state)' * γ/s * ∂g∂ʳvel(contact, state, Δt) end -#Friction + +##Friction @inline function calcFrictionForce!(mechanism::Mechanism{T}, friction::Contact, body::Body, γ) where T cf = friction.cf D = friction.D diff --git a/src/main_components/inequalityconstraint.jl b/src/main_components/inequalityconstraint.jl index 90b5cadf..360df93f 100644 --- a/src/main_components/inequalityconstraint.jl +++ b/src/main_components/inequalityconstraint.jl @@ -57,7 +57,7 @@ end @inline function NtγTof!(mechanism, body::Body, ineqc::InequalityConstraint{T,N}) where {T,N} if isactive(ineqc) state = body.state - state.d -= ∂g∂ʳpos(mechanism, ineqc, body)' * ineqc.γsol[2] + state.d -= ∂g∂ʳpos(mechanism, ineqc, body.id)' * ineqc.γsol[2] for i=1:N state.d -= additionalforce(ineqc.constraints[i]) end @@ -108,13 +108,13 @@ function schurD(ineqc::InequalityConstraint{T,N}, body, Δt) where {T,N} return val end -@generated function ∂g∂ʳpos(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} - vec = [:(∂g∂ʳpos(ineqc.constraints[$i], body)) for i = 1:N] +@generated function ∂g∂ʳpos(mechanism, ineqc::InequalityConstraint{T,N}, id::Integer) where {T,N} + vec = [:(∂g∂ʳpos(ineqc.constraints[$i], getbody(mechanism, id))) for i = 1:N] return :(vcat($(vec...))) end -@generated function ∂g∂ʳvel(mechanism, ineqc::InequalityConstraint{T,N}, body) where {T,N} - vec = [:(∂g∂ʳvel(ineqc.constraints[$i], body, mechanism.Δt)) for i = 1:N] +@generated function ∂g∂ʳvel(mechanism, ineqc::InequalityConstraint{T,N}, id::Integer) where {T,N} + vec = [:(∂g∂ʳvel(ineqc.constraints[$i], getbody(mechanism, id), mechanism.Δt)) for i = 1:N] return :(vcat($(vec...))) end diff --git a/src/solver/solverfunctions.jl b/src/solver/solverfunctions.jl index 05940798..5210d80a 100644 --- a/src/solver/solverfunctions.jl +++ b/src/solver/solverfunctions.jl @@ -205,21 +205,21 @@ function solve!(mechanism::Mechanism) for childid in ineqchildren(graph, id) isinactive(graph, childid) && continue - eliminatedsolve!(mechanism, getineqentry(ldu, childid), diagonal, getbody(mechanism, id), getineqconstraint(mechanism, childid)) + eliminatedsolve!(mechanism, getineqentry(ldu, childid), diagonal, id, getineqconstraint(mechanism, childid)) end end return end -function eliminatedsolve!(mechanism::Mechanism, ineqentry::InequalityEntry, diagonal::DiagonalEntry, body::Body, ineqc::InequalityConstraint) +function eliminatedsolve!(mechanism::Mechanism, ineqentry::InequalityEntry, diagonal::DiagonalEntry, bodyid::Integer, ineqc::InequalityConstraint) Δt = mechanism.Δt μ = mechanism.μ φ = g(mechanism, ineqc) - Nx = ∂g∂ʳpos(mechanism, ineqc, body) - Nv = ∂g∂ʳvel(mechanism, ineqc, body) + Nx = ∂g∂ʳpos(mechanism, ineqc, bodyid) + Nv = ∂g∂ʳvel(mechanism, ineqc, bodyid) γ1 = ineqc.γsol[2] s1 = ineqc.ssol[2] From b574f43766105c2b9b11dadab2a377824feabbb3 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Sun, 13 Dec 2020 22:36:05 +0100 Subject: [PATCH 4/7] Restructure --- src/bounds/bound.jl | 21 +++++--- src/bounds/contact.jl | 38 ++++++++++---- src/bounds/friction.jl | 26 ++++++++++ src/discretization/ImplicitTrapezoid.jl | 2 - src/discretization/SymplecticEuler.jl | 2 - .../implicit_trapezoid/bound.jl | 48 ------------------ src/discretization/symplectic_euler/bound.jl | 50 ------------------- src/joints/genericjoint.jl | 1 + src/joints/joint.jl | 1 + 9 files changed, 70 insertions(+), 119 deletions(-) delete mode 100644 src/discretization/implicit_trapezoid/bound.jl delete mode 100644 src/discretization/symplectic_euler/bound.jl diff --git a/src/bounds/bound.jl b/src/bounds/bound.jl index 15ca2cb1..5f46bb95 100644 --- a/src/bounds/bound.jl +++ b/src/bounds/bound.jl @@ -4,15 +4,24 @@ 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) - -@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) +### Constraints and derivatives +## Position level constraint wrappers +g(contact::Bound, body::Body, Δt) = g(contact, body.state, Δt) +## Additional force for friction default @inline additionalforce(bound::Bound{T}) where T = szeros(T, 6) + +# @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) + + + diff --git a/src/bounds/contact.jl b/src/bounds/contact.jl index 1cabdec5..2c8f663b 100644 --- a/src/bounds/contact.jl +++ b/src/bounds/contact.jl @@ -1,13 +1,24 @@ abstract type Contact{T} <: Bound{T} end -###Constraints and Derivatives -##Position level constraint wrappers -g(contact::Contact, body::Body, Δt) = g(contact, body.state, Δt) -@inline g(contact::Contact{T}) where {T} = szeros(T,6) +### Constraints and derivatives +## Discrete-time position wrappers (for dynamics) +@inline g(contact::Contact, state::State, Δt) = g(contact, posargsnext(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.Nx * (x + vrotate(contact.p,q) - contact.offset) -##Discrete Time Position Derivatives (for dynamics) -#Wrappers 1 + +## Derivatives NOT accounting for quaternion specialness +@inline function ∂g∂pos(contact::Contact, x::AbstractVector, q::UnitQuaternion) + p = contact.p + X = contact.Nx + Q = contact.Nx * (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 @@ -24,8 +35,8 @@ end return [X Q] end -##Discrete Time Velocity Derivatives -#Wrappers 1 +## 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 @@ -42,15 +53,20 @@ end 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)[1: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 - +@inline function schurD(contact::Contact,state::State, γ, s, Δt) + return ∂g∂ʳpos(contact, state)' * γ/s * ∂g∂ʳvel(contact, state, Δt) +end diff --git a/src/bounds/friction.jl b/src/bounds/friction.jl index 2ab7ae4c..4b42b996 100644 --- a/src/bounds/friction.jl +++ b/src/bounds/friction.jl @@ -36,4 +36,30 @@ end @inline function calcFrictionForce!(mechanism, ineqc, friction::Friction, i, body::Body) calcFrictionForce!(mechanism, friction, body, ineqc.γsol[2][i]) return +end + +##Friction +@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] = szeros(T, 3) + state.ωsol[2] = szeros(T, 3) + dyn = dynamics(mechanism, body) + state.vsol[2] = v + ω = state.ωsol[2] = ω + state.d = d + + b0 = D*dyn + friction.b # remove old friction force + + if norm(b0) > cf*γ + friction.b = b0/norm(b0)*cf*γ + else + friction.b = b0 + end + return end \ No newline at end of file diff --git a/src/discretization/ImplicitTrapezoid.jl b/src/discretization/ImplicitTrapezoid.jl index 2d2f17f1..781189b3 100644 --- a/src/discretization/ImplicitTrapezoid.jl +++ b/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")) \ No newline at end of file diff --git a/src/discretization/SymplecticEuler.jl b/src/discretization/SymplecticEuler.jl index d20ba128..78061c6a 100644 --- a/src/discretization/SymplecticEuler.jl +++ b/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")) \ No newline at end of file diff --git a/src/discretization/implicit_trapezoid/bound.jl b/src/discretization/implicit_trapezoid/bound.jl deleted file mode 100644 index 19c8f6c2..00000000 --- a/src/discretization/implicit_trapezoid/bound.jl +++ /dev/null @@ -1,48 +0,0 @@ -@inline g(contact::Contact, x) = contact.Nx[SA[1; 2; 3]]' * (x - contact.offset[SA[1; 2; 3]]) -@inline g(contact::Contact, state::State, Δt) = g(contact, getx3(state, Δt)) - -@inline ∂g∂pos(contact::Contact) = contact.Nx -@inline ∂g∂pos(contact::Contact, state::State) = ∂g∂pos(contact) - -@inline ∂g∂vel(contact::Contact, Δt) = contact.Nx * Δt -@inline ∂g∂vel(contact::Contact, state::State, Δt) = ∂g∂vel(contact, Δt) - -@inline schurf(contact::Contact, φ, γ, s, μ) = ∂g∂pos(contact)' * (γ / s * φ - μ / s) -@inline function schurf(contact::Contact, state::State, γ, s, μ, Δt) - φ = g(contact, getx3(state, Δt)) - return schurf(contact, φ, γ, s, μ) -end - -@inline function schurD(contact::Contact, γ, s, Δt) - Nx = ∂g∂pos(contact) - Nv = ∂g∂vel(contact, Δt) - - return Nx' * γ / s * Nv -end -@inline schurD(contact::Contact, state::State, γ, s, Δt) = schurD(contact, γ, s, Δt) - - -@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] = szeros(T, 3) - state.ωsol[2] = szeros(T, 3) - dyn = dynamics(mechanism, body) - state.vsol[2] = v - ω = state.ωsol[2] = ω - state.d = d - - b0 = D*dyn + friction.b # remove old friction force - - if norm(b0) > cf*γ - friction.b = b0/norm(b0)*cf*γ - else - friction.b = b0 - end - return -end \ No newline at end of file diff --git a/src/discretization/symplectic_euler/bound.jl b/src/discretization/symplectic_euler/bound.jl deleted file mode 100644 index 26d49e1c..00000000 --- a/src/discretization/symplectic_euler/bound.jl +++ /dev/null @@ -1,50 +0,0 @@ -@inline g(contact::Contact, x, q) = contact.Nx * (x + vrotate(contact.p,q) - contact.offset) -@inline g(contact::Contact, state::State, Δt) = g(contact, getx3(state, Δt), getq3(state, Δt)) - -##Discrete Time Position Derivatives -@inline function ∂g∂pos(contact::Contact, x::AbstractVector, q::UnitQuaternion) - p = contact.p - X = contact.Nx - Q = contact.Nx * (VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p))) - return X, Q -end - - -##Schurf und SchurD -#Schurf -@inline function schurf(contact::Contact, state::State, γ, s, μ, Δt) - φ = g(contact, getx3(state, Δt), getq3(state, Δt)) - return ∂g∂ʳpos(contact, state)' * (γ / s * φ - μ / s) -end - -#SchurD -@inline function schurD(contact::Contact,state::State, γ, s, Δt) - return ∂g∂ʳpos(contact, state)' * γ/s * ∂g∂ʳvel(contact, state, Δt) -end - - -##Friction -@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] = szeros(T, 3) - state.ωsol[2] = szeros(T, 3) - dyn = dynamics(mechanism, body) - state.vsol[2] = v - ω = state.ωsol[2] = ω - state.d = d - - b0 = D*dyn + friction.b # remove old friction force - - if norm(b0) > cf*γ - friction.b = b0/norm(b0)*cf*γ - else - friction.b = b0 - end - return -end \ No newline at end of file diff --git a/src/joints/genericjoint.jl b/src/joints/genericjoint.jl index 89bd87c8..b9291381 100644 --- a/src/joints/genericjoint.jl +++ b/src/joints/genericjoint.jl @@ -1,3 +1,4 @@ +# User-defined joint mutable struct GenericJoint{T,N} <: AbstractJoint{T,N} F::SVector{3,T} τ::SVector{3,T} diff --git a/src/joints/joint.jl b/src/joints/joint.jl index a7af6d66..6d68ef7c 100644 --- a/src/joints/joint.jl +++ b/src/joints/joint.jl @@ -1,3 +1,4 @@ +# Standard translational or rotational joint abstract type Joint{T,N} <: AbstractJoint{T,N} end Joint0 = Joint{T,0} where T From b21a45581a0697e13e214a053332779ce97961ff Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Mon, 14 Dec 2020 00:29:08 +0100 Subject: [PATCH 5/7] Initial friction fix try --- examples/dice.jl | 5 ++-- src/bounds/bound.jl | 3 --- src/bounds/contact.jl | 13 +++++++--- src/bounds/friction.jl | 27 +++++++++++++-------- src/bounds/impact.jl | 15 +++++++----- src/main_components/inequalityconstraint.jl | 2 +- test/examples/dice_nofriction.jl | 15 +++--------- 7 files changed, 42 insertions(+), 38 deletions(-) diff --git a/examples/dice.jl b/examples/dice.jl index d3e25f01..4184c401 100644 --- a/examples/dice.jl +++ b/examples/dice.jl @@ -31,16 +31,17 @@ origin = Origin{Float64}() link1 = Body(box1) # Constraints -impacts = [InequalityConstraint(Impact(link1,[0;0;1.0]; p = corners[i])) 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)) +links = [link1] eqcs = [joint0to1] ineqcs = impacts shapes = [box1] -mech = Mechanism(origin, [link1], eqcs, ineqcs, shapes = shapes) +mech = Mechanism(origin, links, eqcs, ineqcs, shapes = shapes) setPosition!(link1,x = [0.;-2;1.5]) ωtemp = (rand(3) .- 0.5) * 100 diff --git a/src/bounds/bound.jl b/src/bounds/bound.jl index 5f46bb95..0b48b9bf 100644 --- a/src/bounds/bound.jl +++ b/src/bounds/bound.jl @@ -12,9 +12,6 @@ end ## Position level constraint wrappers g(contact::Bound, body::Body, Δt) = g(contact, body.state, Δt) -## Additional force for friction default -@inline additionalforce(bound::Bound{T}) where T = szeros(T, 6) - # @inline g(bound::Bound{T}) where T = zero(T) diff --git a/src/bounds/contact.jl b/src/bounds/contact.jl index 2c8f663b..835a4987 100644 --- a/src/bounds/contact.jl +++ b/src/bounds/contact.jl @@ -6,14 +6,14 @@ abstract type Contact{T} <: Bound{T} end ## Position level constraints (for dynamics) # @inline g(contact::Contact{T}) where {T} = szeros(T,6) -@inline g(contact::Contact, x::AbstractVector, q::UnitQuaternion) = contact.Nx * (x + vrotate(contact.p,q) - contact.offset) +@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.Nx - Q = contact.Nx * (VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p))) + X = contact.ainv3 + Q = contact.ainv3 * (VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p))) return X, Q end @@ -56,7 +56,7 @@ 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)[1:6] + 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)...) @@ -70,3 +70,8 @@ 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) \ No newline at end of file diff --git a/src/bounds/friction.jl b/src/bounds/friction.jl index 4b42b996..04998e98 100644 --- a/src/bounds/friction.jl +++ b/src/bounds/friction.jl @@ -1,7 +1,7 @@ mutable struct Friction{T} <: Contact{T} - Nx::Adjoint{T,SVector{3,T}} + ainv3::Adjoint{T,SVector{3,T}} p::SVector{3,T} - D::SMatrix{2,6,T,12} + D::SMatrix{2,3,T,6} cf::T offset::SVector{3,T} b::SVector{2,T} @@ -14,25 +14,32 @@ mutable struct Friction{T} <: Contact{T} 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' - D = [[V1 V2];szeros(3,2)]' + ainv3 = Ainv[3,SA[1; 2; 3]]' + D = [V1 V2]' - new{T}(Nx, p, 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 additionalforce(friction::Friction) = friction.D'*friction.b +@inline function frictionmap(friction::Friction, x::AbstractVector, q::UnitQuaternion) + return [I;VLᵀmat(q)*RVᵀmat(q)*skew(friction.p)] * friction.D' +end +@inline function invfrictionmap(friction::Friction, x::AbstractVector, q::UnitQuaternion) + return friction.D * [I [friction.p';friction.p';friction.p']*VRᵀmat(q)*LVᵀmat(q)] +end + +@inline additionalforce(friction::Friction, x::AbstractVector, q::UnitQuaternion) = frictionmap(friction, x, q)*friction.b + @inline function calcFrictionForce!(mechanism, ineqc, friction::Friction, i, body::Body) calcFrictionForce!(mechanism, friction, body, ineqc.γsol[2][i]) return @@ -51,10 +58,10 @@ end state.ωsol[2] = szeros(T, 3) dyn = dynamics(mechanism, body) state.vsol[2] = v - ω = state.ωsol[2] = ω + state.ωsol[2] = ω state.d = d - b0 = D*dyn + friction.b # remove old friction force + b0 = invfrictionmap(friction, posargsk(body.state)...)*dyn + friction.b # remove old friction force if norm(b0) > cf*γ friction.b = b0/norm(b0)*cf*γ diff --git a/src/bounds/impact.jl b/src/bounds/impact.jl index 0c8364b1..3fc4ea77 100644 --- a/src/bounds/impact.jl +++ b/src/bounds/impact.jl @@ -1,6 +1,6 @@ mutable struct Impact{T} <: Contact{T} - Nx::Adjoint{T,SVector{3,T}} + ainv3::Adjoint{T,SVector{3,T}} p::SVector{3,T} offset::SVector{3,T} @@ -10,16 +10,19 @@ mutable struct Impact{T} <: Contact{T} 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' + ainv3 = Ainv[3,SA[1; 2; 3]]' - new{T}(Nx, p, 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 \ No newline at end of file +end + + +## Additional force for friction default +@inline additionalforce(bound::Impact{T}, x::AbstractVector, q::UnitQuaternion) where T = szeros(T, 6) \ No newline at end of file diff --git a/src/main_components/inequalityconstraint.jl b/src/main_components/inequalityconstraint.jl index 360df93f..e5366d82 100644 --- a/src/main_components/inequalityconstraint.jl +++ b/src/main_components/inequalityconstraint.jl @@ -59,7 +59,7 @@ end state = body.state state.d -= ∂g∂ʳpos(mechanism, ineqc, body.id)' * ineqc.γsol[2] for i=1:N - state.d -= additionalforce(ineqc.constraints[i]) + state.d -= additionalforce(ineqc.constraints[i], body) end end return diff --git a/test/examples/dice_nofriction.jl b/test/examples/dice_nofriction.jl index adfe4397..f93cf1de 100644 --- a/test/examples/dice_nofriction.jl +++ b/test/examples/dice_nofriction.jl @@ -24,26 +24,17 @@ corners = [ origin = Origin{Float64}() link1 = Body(box1) -links = [link1;[Body(b1) for i = 1:8]] +links = [link1] # Constraints -ineqcs = [InequalityConstraint(Impact(links[i + 1], [0;0;1.0])) for i = 1:8] +ineqcs = [InequalityConstraint(Impact(link1, [0;0;1.0], 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]] +eqcs = [EqualityConstraint(Floating(origin, link1))] shapes = [box1;b1] 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 setVelocity!(link1,v = [0;3;7.],ω = [0.1;0.1;0.1]) -for i = 1:8 - setVelocity!(link1, links[i + 1], p1 = corners[i]) -end - - From 5d557a6576c1e1e9306417a6bfe1023b72fe33c1 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Thu, 17 Dec 2020 22:07:50 +0100 Subject: [PATCH 6/7] Update friction (potentially broken) --- examples/dice.jl | 4 +-- src/bounds/friction.jl | 31 +++++++++++------ test/examples/dice.jl | 19 ++++------ test/examples/dice_tiltedplane.jl | 21 ++++------- test/examples/football.jl | 58 ------------------------------- 5 files changed, 35 insertions(+), 98 deletions(-) delete mode 100644 test/examples/football.jl diff --git a/examples/dice.jl b/examples/dice.jl index 4184c401..c9792ad7 100644 --- a/examples/dice.jl +++ b/examples/dice.jl @@ -10,7 +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 Vector v +# Corner vectors corners = [ [[length1 / 2;length1 / 2;-length1 / 2]] [[length1 / 2;-length1 / 2;-length1 / 2]] @@ -48,5 +48,5 @@ setPosition!(link1,x = [0.;-2;1.5]) setVelocity!(link1,v = [0;3;7.],ω = ωtemp) -storage = simulate!(mech, 10., record = true) +storage = simulate!(mech, 10, record = true) visualize(mech, storage, shapes) diff --git a/src/bounds/friction.jl b/src/bounds/friction.jl index 04998e98..f18cd498 100644 --- a/src/bounds/friction.jl +++ b/src/bounds/friction.jl @@ -30,15 +30,25 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::Frictio 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) - return [I;VLᵀmat(q)*RVᵀmat(q)*skew(friction.p)] * friction.D' -end -@inline function invfrictionmap(friction::Friction, x::AbstractVector, q::UnitQuaternion) - return friction.D * [I [friction.p';friction.p';friction.p']*VRᵀmat(q)*LVᵀmat(q)] + 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)*friction.b +@inline additionalforce(friction::Friction, x::AbstractVector, q::UnitQuaternion) = frictionmap(friction, x, q) @inline function calcFrictionForce!(mechanism, ineqc, friction::Friction, i, body::Body) calcFrictionForce!(mechanism, friction, body, ineqc.γsol[2][i]) @@ -46,6 +56,7 @@ end 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 @@ -54,15 +65,15 @@ end d = state.d v = state.vsol[2] ω = state.ωsol[2] - state.vsol[2] = szeros(T, 3) - state.ωsol[2] = szeros(T, 3) - dyn = dynamics(mechanism, body) + 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 = invfrictionmap(friction, posargsk(body.state)...)*dyn + friction.b # remove old friction force - + 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 diff --git a/test/examples/dice.jl b/test/examples/dice.jl index 3f54222a..6d892ca5 100644 --- a/test/examples/dice.jl +++ b/test/examples/dice.jl @@ -9,6 +9,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]] @@ -22,29 +23,21 @@ corners = [ # 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 ωtemp = [0.1;0.1;0.1] setVelocity!(link1,v = [0;3;7.],ω = ωtemp) -for i = 1:8 - setVelocity!(link1, links[i + 1], p1 = corners[i]) -end - diff --git a/test/examples/dice_tiltedplane.jl b/test/examples/dice_tiltedplane.jl index 0c81293e..784ffc33 100644 --- a/test/examples/dice_tiltedplane.jl +++ b/test/examples/dice_tiltedplane.jl @@ -9,6 +9,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]] @@ -22,31 +23,21 @@ corners = [ # 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;1.0], cf)) for i = 1:8] +impacts = [InequalityConstraint(Friction(link1,[0;-0.1;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 ωtemp = [0.1;0.1;0.1] -# ω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 diff --git a/test/examples/football.jl b/test/examples/football.jl deleted file mode 100644 index 1e9e3216..00000000 --- a/test/examples/football.jl +++ /dev/null @@ -1,58 +0,0 @@ -using ConstrainedDynamics -using LinearAlgebra -using StaticArrays - - -# Parameters -h = 0.06 -r = 0.08 -b1 = Cylinder(r*0.4, h, h*(r*0.4)^2*500, color = RGBA(0.8, 0.2, 0.)) -b2 = Cylinder(r*0.8, h, h*(r*0.8)^2*500, color = RGBA(0.8, 0.2, 0.)) -b3 = Cylinder(r, h, h*r^2*500, color = RGBA(0.8, 0.2, 0.)) - -# Links -origin = Origin{Float64}() -link1 = Body(b1) -link2 = Body(b2) -link3 = Body(b3) -link4 = Body(b2) -link5 = Body(b1) - - -# Constraints -joint1 = EqualityConstraint(Fixed(link2, link1; p1=-[0;0;0.03], p2=[0;0;0.03])) -joint2 = EqualityConstraint(Fixed(link3, link2; p1=-[0;0;0.03], p2=[0;0;0.03])) -joint3 = EqualityConstraint(Floating(origin, link3)) -joint4 = EqualityConstraint(Fixed(link3, link4; p1=[0;0;0.03], p2=-[0;0;0.03])) -joint5 = EqualityConstraint(Fixed(link4, link5; p1=[0;0;0.03], p2=-[0;0;0.03])) -fr1 = InequalityConstraint(Friction(link1, [0;0;1.0], 0.6)) -fr2 = InequalityConstraint(Friction(link2, [0;0;1.0], 0.6)) -fr3 = InequalityConstraint(Friction(link3, [0;0;1.0], 0.6)) -fr4 = InequalityConstraint(Friction(link4, [0;0;1.0], 0.6)) -fr5 = InequalityConstraint(Friction(link5, [0;0;1.0], 0.6)) - -links = [link1;link2;link3;link4;link5] -constraints = [joint1;joint2;joint3;joint4;joint5] -fr = [fr1;fr2;fr3;fr4;fr5] -shapes = [b1;b2;b3] - - -mech = Mechanism(origin, links, constraints, fr, shapes = shapes) -setPosition!(link3,x = [0.;-10.0;1.5],q=UnitQuaternion(RotX(pi/2))) -setPosition!(link3,link2,Δx = -[0.;0.;0.03]) -setPosition!(link2,link1,Δx = -[0.;0.;0.03]) -setPosition!(link3,link4,Δx = [0.;0.;0.03]) -setPosition!(link4,link5,Δx = [0.;0.;0.03]) - -spin = 0.35 - -function football_control!(mechanism, k) - if k<25 - setForce!(link3, F = SA[0.;25.;25.] * 0, τ=spin*SA[0.2;0.2;1.] * 0) - elseif k==40 - setForce!(link3, τ = SA[0.;0.3;0.] * 0) - else - setForce!(link3) - end - return -end From 7335bfa512d69fcece88e651698eaccb39995528 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Thu, 17 Dec 2020 22:13:27 +0100 Subject: [PATCH 7/7] Update tests --- benchmark/example_benchmark.jl | 2 -- test/example_test.jl | 2 -- 2 files changed, 4 deletions(-) diff --git a/benchmark/example_benchmark.jl b/benchmark/example_benchmark.jl index 00179177..7aaa6c44 100644 --- a/benchmark/example_benchmark.jl +++ b/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" ] diff --git a/test/example_test.jl b/test/example_test.jl index 4addd8f7..7f081df7 100644 --- a/test/example_test.jl +++ b/test/example_test.jl @@ -12,7 +12,6 @@ files = [ "doublependulum_disconnection" "doublependulum_urdf" "elliptic_joint" - "football" "fourbar_disconnection" "inverted_pyramid_plane" "joint_force" @@ -36,7 +35,6 @@ controlled = [ "joint_torque" "pendulum_forced" "nutation" - "football" "fourbar_disconnection" "slider_crank" ]