From 036b098518ab6c0412476b8b50283fcfdcb95ba6 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Wed, 12 Apr 2023 17:13:49 +0200 Subject: [PATCH 1/6] Remove matrix_entries from mechanism --- src/mechanism/constructor.jl | 5 +---- src/solver/linear_system.jl | 10 ---------- src/solver/mehrotra.jl | 2 -- 3 files changed, 1 insertion(+), 16 deletions(-) diff --git a/src/mechanism/constructor.jl b/src/mechanism/constructor.jl index cbc188e77..fcfede7f6 100644 --- a/src/mechanism/constructor.jl +++ b/src/mechanism/constructor.jl @@ -9,7 +9,6 @@ contacts: list of ContactConstraint objects system: graph-based representation for mechanism residual_entries: containt entries for linear system residual - matrix_entries: contains entries for linear system matrix data_matrix: contains parameter information that is fixed during simulation root_to_leaves: list of node connections traversing from root node to leaves timestep: time discretization @@ -25,7 +24,6 @@ mutable struct Mechanism{T,Nn,Ne,Nb,Ni} system::System{Nn} residual_entries::Vector{Entry} - matrix_entries::SparseMatrixCSC{Entry,Int64} data_matrix::SparseMatrixCSC{Entry,Int64} root_to_leaves::Vector{Int64} @@ -70,7 +68,6 @@ function Mechanism(origin::Origin{T}, bodies::Vector{Body{T}}, joints::Vector{<: # graph system system = create_system(origin, joints, bodies, contacts) residual_entries = deepcopy(system.vector_entries) - matrix_entries = deepcopy(system.matrix_entries) # data gradient system data_matrix = create_data_matrix(joints, bodies, contacts) @@ -83,7 +80,7 @@ function Mechanism(origin::Origin{T}, bodies::Vector{Body{T}}, joints::Vector{<: exclude_loop_joints=false) Mechanism{T,Nn,Ne,Nb,Ni}(origin, joints, bodies, contacts, system, residual_entries, - matrix_entries, data_matrix, root_to_leaves, timestep, input_scaling, get_gravity(gravity), 0.0) + data_matrix, root_to_leaves, timestep, input_scaling, get_gravity(gravity), 0.0) end Mechanism(origin::Origin{T}, bodies::Vector{Body{T}}, joints::Vector{<:JointConstraint{T}}; kwargs...) where T = diff --git a/src/solver/linear_system.jl b/src/solver/linear_system.jl index d62c03ce5..e86f65434 100644 --- a/src/solver/linear_system.jl +++ b/src/solver/linear_system.jl @@ -43,16 +43,6 @@ function push_residual!(mechanism::Mechanism) return end -function pull_matrix!(mechanism::Mechanism) - mechanism.matrix_entries.nzval .= mechanism.system.matrix_entries.nzval #TODO: make allocation free - return -end - -function push_matrix!(mechanism::Mechanism) - mechanism.system.matrix_entries.nzval .= mechanism.matrix_entries.nzval #TODO: make allocation free - return -end - function update!(body::Body) body.state.vsol[1] = body.state.vsol[2] body.state.ωsol[1] = body.state.ωsol[2] diff --git a/src/solver/mehrotra.jl b/src/solver/mehrotra.jl index e99c2d8bb..2963055ac 100644 --- a/src/solver/mehrotra.jl +++ b/src/solver/mehrotra.jl @@ -34,7 +34,6 @@ function mehrotra!(mechanism::Mechanism; opts=SolverOptions()) μ = 0.0 pull_residual!(mechanism) # store the residual inside mechanism.residual_entries ldu_factorization!(mechanism.system) # factorize system, modifies the matrix in place - pull_matrix!(mechanism) # store the factorized matrix inside mechanism.matrix_entries ldu_backsubstitution!(mechanism.system) # solve system, modifies the vector in place αaff = cone_line_search!(mechanism; τort=0.95, τsoc=0.95, scaling=false) # uses system.vector_entries which holds the search drection @@ -47,7 +46,6 @@ function mehrotra!(mechanism::Mechanism; opts=SolverOptions()) correction!(mechanism) # update the residual in mechanism.residual_entries push_residual!(mechanism) # cache residual + correction - push_matrix!(mechanism) # restore the factorized matrix ldu_backsubstitution!(mechanism.system) # solve system τ = max(0.95, 1 - max(rvio, bvio)^2) # τ = 0.95 From 6df07a5ab7e59e52da9fbe31568c6a19dc2931b6 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Wed, 12 Apr 2023 19:21:41 +0200 Subject: [PATCH 2/6] Some allocation reductions --- src/joints/constraints.jl | 28 ++++++++++++++++++---------- src/joints/translational/input.jl | 8 ++++---- src/mechanics/momentum.jl | 23 +++++++++++++---------- src/simulation/storage.jl | 8 +++----- src/solver/complementarity.jl | 23 +++++++++++------------ src/solver/line_search.jl | 21 +++++++++++++-------- src/solver/linear_system.jl | 12 ++++++++++-- src/solver/mehrotra.jl | 6 +++--- src/solver/violations.jl | 30 +++++++++++++++++++++--------- 9 files changed, 96 insertions(+), 63 deletions(-) diff --git a/src/joints/constraints.jl b/src/joints/constraints.jl index 8ae759a66..d85ea6c0d 100644 --- a/src/joints/constraints.jl +++ b/src/joints/constraints.jl @@ -99,16 +99,24 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::JointCo 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)) +# @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) + 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 # # constraints Jacobians diff --git a/src/joints/translational/input.jl b/src/joints/translational/input.jl index 35dce6970..a703b4771 100644 --- a/src/joints/translational/input.jl +++ b/src/joints/translational/input.jl @@ -12,10 +12,10 @@ function input_impulse!(joint::Translational{T}, input = joint.input * input_scaling Ta = impulse_transform(:parent, joint, xa, qa, xb, qb) Tb = impulse_transform(:child, joint, xa, qa, xb, qb) - JFaw = Ta[1:3,1:3] * input - Jτaa = Ta[4:6,1:3] * input - JFbw = Tb[1:3,1:3] * input - Jτbb = Tb[4:6,1:3] * input + JFaw = Ta[SA[1;2;3],SA[1;2;3]] * input + Jτaa = Ta[SA[4;5;6],SA[1;2;3]] * input + JFbw = Tb[SA[1;2;3],SA[1;2;3]] * input + Jτbb = Tb[SA[4;5;6],SA[1;2;3]] * input pbody.state.JF2 += JFaw pbody.state.Jτ2 += Jτaa/2 diff --git a/src/mechanics/momentum.jl b/src/mechanics/momentum.jl index 47c2e7898..8bbcd4fda 100644 --- a/src/mechanics/momentum.jl +++ b/src/mechanics/momentum.jl @@ -32,20 +32,23 @@ function momentum(mechanism::Mechanism{T}, body::Body{T}) where T p_angular_body = D2q - 0.5 * state.Jτ2 for joint in mechanism.joints - if body.id ∈ (joint.parent_id, joint.child_id) + f_joint = joint_impulses(mechanism, joint, body) + p_linear_body -= 0.5 * f_joint[SA[1;2;3]] + p_angular_body -= 0.5 * f_joint[SA[4;5;6]] + end - f_joint = szeros(T,6) - (length(joint) > 0) && (f_joint += impulse_map(mechanism, joint, body) * joint.impulses[2]) # computed at t = 1.5 + return p_linear_body, vector_rotate(p_angular_body, q2) +end - joint.spring && (f_joint += spring_impulses(mechanism, joint, body)) # computed at t = 1.5 - joint.damper && (f_joint += damper_impulses(mechanism, joint, body)) # computed at t = 1.5 +function joint_impulses(mechanism, joint::JointConstraint{T,N}, body) where {T,N} + f_joint = szeros(T,6) + if body.id ∈ (joint.parent_id, joint.child_id) + (N > 0) && (f_joint += impulse_map(mechanism, joint, body) * joint.impulses[2]) # computed at t = 1.5 - p_linear_body -= 0.5 * f_joint[1:3] - p_angular_body -= 0.5 * f_joint[4:6] - end + joint.spring && (f_joint += spring_impulses(mechanism, joint, body)) # computed at t = 1.5 + joint.damper && (f_joint += damper_impulses(mechanism, joint, body)) # computed at t = 1.5 end - - return [p_linear_body; rotation_matrix(q2) * p_angular_body] + return f_joint end function momentum(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, storage::Storage{T,Ns}, t::Int) where {T,Nn,Ne,Nb,Ni,Ns} diff --git a/src/simulation/storage.jl b/src/simulation/storage.jl index 3fbec7604..64f4594ff 100644 --- a/src/simulation/storage.jl +++ b/src/simulation/storage.jl @@ -55,11 +55,9 @@ function save_to_storage!(mechanism::Mechanism, storage::Storage, i::Int) storage.v[ind][i] = state.v15 # v1.5 storage.ω[ind][i] = state.ω15 # ω1.5 q2 = state.q2 - p2 = momentum(mechanism, body) # p1 in world frame - px2 = p2[SVector{3,Int}(1,2,3)] # px1 in world frame - pq2 = p2[SVector{3,Int}(4,5,6)] # pq1 in world frame - v2 = px2 ./ body.mass # in world frame - ω2 = body.inertia \ (rotation_matrix(inv(q2)) * pq2) # in body frame, we rotate using the current quaternion q2 = state.q2 + px2, pq2 = momentum(mechanism, body) # p in world frame + v2 = px2 / body.mass # in world frame + ω2 = body.inertia \ (vector_rotate(pq2, inv(q2))) # in body frame, we rotate using the current quaternion q2 = state.q2 storage.px[ind][i] = px2 # px2 storage.pq[ind][i] = pq2 # pq2 storage.vl[ind][i] = v2 # v2 diff --git a/src/solver/complementarity.jl b/src/solver/complementarity.jl index e1b6ad973..286057df1 100644 --- a/src/solver/complementarity.jl +++ b/src/solver/complementarity.jl @@ -1,22 +1,21 @@ # joints -function complementarity(mechanism, joint::JointConstraint{T,N,Nc}; - scaling::Bool=false) where {T,N,Nc} +function complementarity(mechanism, joint::JointConstraint{T,N,Nc}) where {T,N,Nc} - c = [] - for (i, element) in enumerate((joint.translational, joint.rotational)) - λi = joint.impulses[2][joint_impulse_index(joint, i)] - si, γi = split_impulses(element, λi) - push!(c, si .* γi) - end + λ = joint.impulses[2][joint_impulse_index(joint, 1)] + s, γ = split_impulses(joint.translational, λ) + ct = s .* γ - return vcat(c...) + λ = joint.impulses[2][joint_impulse_index(joint, 2)] + s, γ = split_impulses(joint.rotational, λ) + cr = s .* γ + + return [ct; cr] end # contacts -complementarity(mechanism, contact::ContactConstraint; scaling=false) = contact.impulses[2] .* contact.impulses_dual[2] +complementarity(mechanism, contact::ContactConstraint) = contact.impulses[2] .* contact.impulses_dual[2] -function complementarity(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}; - scaling::Bool = false) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} +function complementarity(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs<:NonlinearContact{T,N}} γ = contact.impulses[2] s = contact.impulses_dual[2] return vcat(γ[1] * s[1], cone_product(γ[@SVector [2,3,4]], s[@SVector [2,3,4]])) diff --git a/src/solver/line_search.jl b/src/solver/line_search.jl index e70ebeda7..d1c63beb2 100644 --- a/src/solver/line_search.jl +++ b/src/solver/line_search.jl @@ -2,7 +2,8 @@ function line_search!(mechanism::Mechanism, α, rvio, bvio, opts) scale = 0 system = mechanism.system - rvio_cand, bvio_cand = Inf * ones(2) + rvio_cand = Inf + bvio_cand = Inf for n = Base.OneTo(opts.max_ls) for contact in mechanism.contacts candidate_step!(α, contact, get_entry(system, contact.id), scale) @@ -86,14 +87,18 @@ function cone_line_search!(α, mechanism, joint::JointConstraint{T,N,Nc}, vector_entry::Entry, τort, τsoc; scaling::Bool=false) where {T,N,Nc} - for (i, element) in enumerate((joint.translational, joint.rotational)) - s, γ = split_impulses(element, joint.impulses[2][joint_impulse_index(joint,i)]) - Δs, Δγ = split_impulses(element, vector_entry.value[joint_impulse_index(joint,i)]) + + s, γ = split_impulses(joint.translational, joint.impulses[2][joint_impulse_index(joint,1)]) + Δs, Δγ = split_impulses(joint.translational, vector_entry.value[joint_impulse_index(joint,1)]) + αs_ort = positive_orthant_step_length(s, Δs, τ = τort) + αγ_ort = positive_orthant_step_length(γ, Δγ, τ = τort) + α = min(α, αs_ort, αγ_ort) - αs_ort = positive_orthant_step_length(s, Δs, τ = τort) - αγ_ort = positive_orthant_step_length(γ, Δγ, τ = τort) - α = min(α, αs_ort, αγ_ort) - end + s, γ = split_impulses(joint.rotational, joint.impulses[2][joint_impulse_index(joint,2)]) + Δs, Δγ = split_impulses(joint.rotational, vector_entry.value[joint_impulse_index(joint,2)]) + αs_ort = positive_orthant_step_length(s, Δs, τ = τort) + αγ_ort = positive_orthant_step_length(γ, Δγ, τ = τort) + α = min(α, αs_ort, αγ_ort) return α end diff --git a/src/solver/linear_system.jl b/src/solver/linear_system.jl index e86f65434..fe501c440 100644 --- a/src/solver/linear_system.jl +++ b/src/solver/linear_system.jl @@ -31,18 +31,26 @@ end function pull_residual!(mechanism::Mechanism) for i in eachindex(mechanism.residual_entries) - mechanism.residual_entries[i].value = mechanism.system.vector_entries[i].value + pull_residual!(mechanism.residual_entries[i], mechanism.system.vector_entries[i]) end return end +function pull_residual!(residual_entry, vector_entry) + residual_entry.value = vector_entry.value +end + function push_residual!(mechanism::Mechanism) for i in eachindex(mechanism.residual_entries) - mechanism.system.vector_entries[i].value = mechanism.residual_entries[i].value + push_residual!(mechanism.system.vector_entries[i], mechanism.residual_entries[i]) end return end +function push_residual!(vector_entry, residual_entry) + vector_entry.value = residual_entry.value +end + function update!(body::Body) body.state.vsol[1] = body.state.vsol[2] body.state.ωsol[1] = body.state.ωsol[2] diff --git a/src/solver/mehrotra.jl b/src/solver/mehrotra.jl index 2963055ac..7b11f1060 100644 --- a/src/solver/mehrotra.jl +++ b/src/solver/mehrotra.jl @@ -6,7 +6,7 @@ mechanism: Mechanism opts: SolverOptions """ -function mehrotra!(mechanism::Mechanism; opts=SolverOptions()) +function mehrotra!(mechanism::Mechanism{T}; opts=SolverOptions{T}()) where T reset!.(mechanism.contacts, scale=1.0) # resets the values of s and γ to the scaled neutral vector; TODO: solver option reset!.(mechanism.joints, scale=1.0) # resets the values of s and γ to the scaled neutral vector; TODO: solver option @@ -17,7 +17,7 @@ function mehrotra!(mechanism::Mechanism; opts=SolverOptions()) undercut = opts.undercut α = 1.0 - initialize!.(mechanism.contacts) # TODO: redundant with resetVars--remove + initialize!.(mechanism.contacts) set_entries!(mechanism) # compute the residual bvio = bilinear_violation(mechanism) # does not require to apply set_entries! @@ -27,7 +27,7 @@ function mehrotra!(mechanism::Mechanism; opts=SolverOptions()) for n = Base.OneTo(opts.max_iter) opts.verbose && solver_status(mechanism, α, rvio, bvio, n, μtarget, undercut) - ((rvio < opts.rtol) && (bvio < opts.btol)) && (status=:success; break) + (rvio < opts.rtol) && (bvio < opts.btol) && (status=:success; break) (n == opts.max_iter) && (opts.verbose && (@warn "failed mehrotra")) # affine search direction diff --git a/src/solver/violations.jl b/src/solver/violations.jl index 0b998dcd8..ca3ea5d00 100644 --- a/src/solver/violations.jl +++ b/src/solver/violations.jl @@ -1,15 +1,7 @@ function residual_violation(mechanism::Mechanism) violation = 0.0 for joint in mechanism.joints - res = constraint(mechanism, joint) - shift = 0 - for element in (joint.translational, joint.rotational) - Nλ = joint_length(element) - Nb = limits_length(element) - subres = res[shift + 2Nb .+ (1:Nλ)] - violation = max(violation, norm(subres, Inf)) - shift += impulses_length(element) - end + violation = max(violation, joint_residual_violation(mechanism, joint)) end for body in mechanism.bodies res = constraint(mechanism, body) @@ -22,6 +14,26 @@ function residual_violation(mechanism::Mechanism) return violation end +# TODO probably could be made alloc free with @generated or dispatching +function joint_residual_violation(mechanism, joint) + res = constraint(mechanism, joint) + + joint_t = joint.translational + Nλ = joint_length(joint_t) + Nb = limits_length(joint_t) + subres = res[2Nb .+ (1:Nλ)] + violation = norm(subres, Inf) + + shift = impulses_length(joint_t) + joint_r = joint.rotational + Nλ = joint_length(joint_r) + Nb = limits_length(joint_r) + subres = res[shift + 2Nb .+ (1:Nλ)] + violation = max(violation, norm(subres, Inf)) + + return violation +end + function bilinear_violation(mechanism::Mechanism) violation = 0.0 for contact in mechanism.contacts From 6d144aa52d8c05954915d26c7e4bdb0a097f0d7d Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Wed, 12 Apr 2023 20:00:10 +0200 Subject: [PATCH 3/6] Remove scaling keyword in solver --- src/solver/complementarity.jl | 2 +- src/solver/line_search.jl | 16 ++++++---------- src/solver/mehrotra.jl | 4 ++-- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/solver/complementarity.jl b/src/solver/complementarity.jl index 286057df1..158212861 100644 --- a/src/solver/complementarity.jl +++ b/src/solver/complementarity.jl @@ -21,4 +21,4 @@ function complementarity(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where return vcat(γ[1] * s[1], cone_product(γ[@SVector [2,3,4]], s[@SVector [2,3,4]])) end -complementarityμ(mechanism, contact::ContactConstraint; scaling=false) = complementarity(mechanism, contact, scaling=scaling) - mechanism.μ * neutral_vector(contact.model) +complementarityμ(mechanism, contact::ContactConstraint) = complementarity(mechanism, contact) - mechanism.μ * neutral_vector(contact.model) diff --git a/src/solver/line_search.jl b/src/solver/line_search.jl index d1c63beb2..d18dc816b 100644 --- a/src/solver/line_search.jl +++ b/src/solver/line_search.jl @@ -35,25 +35,23 @@ end function cone_line_search!(mechanism::Mechanism; τort::T=0.95, - τsoc::T=0.95, - scaling::Bool=false) where T + τsoc::T=0.95) where T system = mechanism.system α = 1.0 for contact in mechanism.contacts - α = cone_line_search!(α, mechanism, contact, get_entry(system, contact.id), τort, τsoc; scaling = scaling) + α = cone_line_search!(α, mechanism, contact, get_entry(system, contact.id), τort, τsoc) end for joint in mechanism.joints - α = cone_line_search!(α, mechanism, joint, get_entry(system, joint.id), τort, τsoc; scaling = scaling) + α = cone_line_search!(α, mechanism, joint, get_entry(system, joint.id), τort, τsoc) end return α end function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, - vector_entry::Entry, τort, τsoc; - scaling::Bool=false) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} + vector_entry::Entry, τort, τsoc) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} s = contact.impulses_dual[2] γ = contact.impulses[2] @@ -68,8 +66,7 @@ function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N end function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, - vector_entry::Entry, τort, τsoc; - scaling::Bool=false) where {T,N,Nc,Cs<:Union{ImpactContact{T,N},LinearContact{T,N}},N½} + vector_entry::Entry, τort, τsoc) where {T,N,Nc,Cs<:Union{ImpactContact{T,N},LinearContact{T,N}},N½} s = contact.impulses_dual[2] γ = contact.impulses[2] @@ -84,8 +81,7 @@ function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N end function cone_line_search!(α, mechanism, joint::JointConstraint{T,N,Nc}, - vector_entry::Entry, τort, τsoc; - scaling::Bool=false) where {T,N,Nc} + vector_entry::Entry, τort, τsoc) where {T,N,Nc} s, γ = split_impulses(joint.translational, joint.impulses[2][joint_impulse_index(joint,1)]) diff --git a/src/solver/mehrotra.jl b/src/solver/mehrotra.jl index 7b11f1060..d9e5c5b1f 100644 --- a/src/solver/mehrotra.jl +++ b/src/solver/mehrotra.jl @@ -36,7 +36,7 @@ function mehrotra!(mechanism::Mechanism{T}; opts=SolverOptions{T}()) where T ldu_factorization!(mechanism.system) # factorize system, modifies the matrix in place ldu_backsubstitution!(mechanism.system) # solve system, modifies the vector in place - αaff = cone_line_search!(mechanism; τort=0.95, τsoc=0.95, scaling=false) # uses system.vector_entries which holds the search drection + αaff = cone_line_search!(mechanism; τort=0.95, τsoc=0.95) # uses system.vector_entries which holds the search drection ν, νaff = centering!(mechanism, αaff) σcentering = clamp(νaff / (ν + 1e-20), 0.0, 1.0)^3 @@ -49,7 +49,7 @@ function mehrotra!(mechanism::Mechanism{T}; opts=SolverOptions{T}()) where T ldu_backsubstitution!(mechanism.system) # solve system τ = max(0.95, 1 - max(rvio, bvio)^2) # τ = 0.95 - α = cone_line_search!(mechanism; τort=τ, τsoc=min(τ, 0.95), scaling=false) # uses system.vector_entries which holds the corrected search drection + α = cone_line_search!(mechanism; τort=τ, τsoc=min(τ, 0.95)) # uses system.vector_entries which holds the corrected search drection # steps taken without making progress rvio_, bvio_ = line_search!(mechanism, α, rvio, bvio, opts) From 022eddd3d0f1f6932a758c9ba8e017a86605fc87 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Thu, 13 Apr 2023 16:28:47 +0200 Subject: [PATCH 4/6] Reduce contact allocations --- src/bodies/shapes.jl | 2 +- src/contacts/collisions/capsule_capsule.jl | 4 +- src/contacts/collisions/collision.jl | 2 +- src/contacts/collisions/point_to_box.jl | 32 +------- src/contacts/collisions/point_to_box_v2.jl | 30 -------- src/contacts/collisions/point_to_segment.jl | 83 +-------------------- src/contacts/collisions/sphere_halfspace.jl | 2 +- src/contacts/cone.jl | 2 +- src/contacts/contact.jl | 2 +- src/contacts/linear.jl | 4 +- src/contacts/nonlinear.jl | 2 +- src/contacts/velocity.jl | 2 +- src/gradients/utilities.jl | 10 +-- src/joints/constraints.jl | 2 +- src/joints/joint.jl | 4 +- src/joints/minimal.jl | 12 +-- src/joints/rotational/dampers.jl | 4 +- src/joints/rotational/impulses.jl | 8 +- src/joints/rotational/springs.jl | 4 +- src/joints/translational/input.jl | 20 ++--- src/joints/translational/springs.jl | 4 +- src/mechanism/data.jl | 16 ++-- src/mechanism/set.jl | 6 +- src/mechanism/state.jl | 14 ++-- src/mechanism/urdf.jl | 2 +- src/orientation/mrp.jl | 2 +- src/orientation/rotate.jl | 6 +- src/simulation/storage.jl | 10 +-- src/solver/centering.jl | 57 ++++++++------ src/solver/complementarity.jl | 2 +- src/solver/correction.jl | 10 +-- src/solver/initialization.jl | 18 ++--- src/solver/line_search.jl | 25 ++++--- src/solver/violations.jl | 39 +++++++--- src/utilities/custom_static.jl | 3 + src/visuals/rope.jl | 6 +- 36 files changed, 170 insertions(+), 281 deletions(-) diff --git a/src/bodies/shapes.jl b/src/bodies/shapes.jl index 9827d3065..b3ed5f641 100644 --- a/src/bodies/shapes.jl +++ b/src/bodies/shapes.jl @@ -326,7 +326,7 @@ mutable struct FrameShape{T} <: Shape{T} scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) T = promote_type(quateltype.((m, position_offset, orientation_offset))...) - J = m * diagm([1;1;1]) + J = m * sI(3) return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, scale, color)) end end diff --git a/src/contacts/collisions/capsule_capsule.jl b/src/contacts/collisions/capsule_capsule.jl index 02abfacf3..df395eb59 100644 --- a/src/contacts/collisions/capsule_capsule.jl +++ b/src/contacts/collisions/capsule_capsule.jl @@ -79,9 +79,9 @@ function contact_point(relative::Symbol, collision::CapsuleCapsuleCollision, xp, # call mehrotra if relative == :parent - return collision.ip.z[0 .+ (1:3)] + return collision.ip.z[SA[1;2;3]] elseif relative == :child - return collision.ip.z[3 .+ (1:3)] + return collision.ip.z[SA[4;5;6]] end end diff --git a/src/contacts/collisions/collision.jl b/src/contacts/collisions/collision.jl index 13acce67d..0d808d543 100644 --- a/src/contacts/collisions/collision.jl +++ b/src/contacts/collisions/collision.jl @@ -10,7 +10,7 @@ function contact_point_origin(x, q, k) x + vector_rotate(k, q) end -∂contact_point_origin∂x(x, q, k) = 1.0 * I(3) +∂contact_point_origin∂x(x, q, k) = 1.0 * sI(3) ∂contact_point_origin∂q(x, q, k) = ∂vector_rotate∂q(k, q) ∂contact_point_origin∂k(x, q, k) = rotation_matrix(q) diff --git a/src/contacts/collisions/point_to_box.jl b/src/contacts/collisions/point_to_box.jl index 6ca14526e..637ee1784 100644 --- a/src/contacts/collisions/point_to_box.jl +++ b/src/contacts/collisions/point_to_box.jl @@ -109,7 +109,7 @@ function ∂contact_point_box∂x(ps, xc, qc, kx, ky, kz) ty = dot(ps - origin, v2) / dot(v2, v2) tz = dot(ps - origin, v3) / dot(v3, v3) - X = 1.0 * I(3) + X = 1.0 * sI(3) if tx >= 1.0 # coc += v1 @@ -205,33 +205,3 @@ function ∂contact_point_box∂q(ps, xc, qc, kx, ky, kz) return Q # FiniteDiff.finite_difference_jacobian(q -> contact_point_box(ps, xc, Quaternion(q...), kx, ky, kz), vector(qc)) end - -# x_min = -1.0 -# x_max = 1.0 -# z_min = -1.0 -# z_max = 1.0 - -# p = [-1.0; 1.0] #.- 1.0e-6 - -# 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 -# end -# end - -# function nearest_point(p, x_min, x_max, z_min, z_max) -# return [klamp(p[1], x_min, x_max), klamp(p[2], z_min, z_max)] -# end - -# nearest_point(p, x_min, x_max, z_min, z_max) \ No newline at end of file diff --git a/src/contacts/collisions/point_to_box_v2.jl b/src/contacts/collisions/point_to_box_v2.jl index d109f45d6..d8bd5bd09 100644 --- a/src/contacts/collisions/point_to_box_v2.jl +++ b/src/contacts/collisions/point_to_box_v2.jl @@ -43,33 +43,3 @@ 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 - -# x_min = -1.0 -# x_max = 1.0 -# z_min = -1.0 -# z_max = 1.0 - -# p = [-1.0; 1.0] #.- 1.0e-6 - -# 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 -# end -# end - -# function nearest_point(p, x_min, x_max, z_min, z_max) -# return [klamp(p[1], x_min, x_max), klamp(p[2], z_min, z_max)] -# end - -# nearest_point(p, x_min, x_max, z_min, z_max) \ No newline at end of file diff --git a/src/contacts/collisions/point_to_segment.jl b/src/contacts/collisions/point_to_segment.jl index 13ab08cb3..a413b936a 100644 --- a/src/contacts/collisions/point_to_segment.jl +++ b/src/contacts/collisions/point_to_segment.jl @@ -125,85 +125,4 @@ function ∂contact_point_segment∂q(ps, xc, qc, ka, kb) return Q end -end - -# # case 1 -# ps = [-2.0 - 0.0e-8; 0.0; 1.0] -# xc = [0.0; 0.0; 0.0] -# qc = Quaternion([1.0; 0.0; 0.0; 0.0]) -# ka = [1.0; 0.0; 0.0] -# kb = [-1.0; 0.0; 0.0] - -# contact_point_segment(ps, xc, qc, ka, kb) - -# ∂contact_point_segment∂p(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps) - -# ∂contact_point_segment∂x(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc) - -# ∂contact_point_segment∂q(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc)) - -# # case 2 -# ps = [1.0 + 100.0e-8; 0.0; 1.0] -# xc = [0.0; 0.0; 0.0] -# qc = Quaternion([1.0; 0.0; 0.0; 0.0]) -# ka = [1.0; 0.0; 0.0] -# kb = [-1.0; 0.0; 0.0] - -# contact_point_segment(ps, xc, qc, ka, kb) - -# ∂contact_point_segment∂p(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps) - -# ∂contact_point_segment∂x(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc) - -# ∂contact_point_segment∂q(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc)) - -# # case 3 -# ps = [0.0; 0.0; 1.0] -# xc = [0.0; 0.0; 0.0] -# qc = Quaternion([1.0; 0.0; 0.0; 0.0]) -# ka = [1.0; 0.0; 0.0] -# kb = [-1.0; 0.0; 0.0] - -# contact_point_segment(ps, xc, qc, ka, kb) - -# ∂contact_point_segment∂p(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps) - -# ∂contact_point_segment∂x(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc) - -# ∂contact_point_segment∂q(ps, xc, qc, ka, kb) -# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc)) - - - -# function get_t(a, b) -# return a' * b ./ (b' * b) -# end - -# function dget_tda(a, b) -# return b' ./ (b' * b) -# end - -# function dget_tdb(a, b) -# D = a' ./ (b' * b) -# D += -1.0 * a' * b ./ (b' * b)^2 * 2.0 * b' -# return D -# end - -# a = rand(3) -# b = rand(3) - -# get_t(a, b) - -# dget_tda(a, b) -# FiniteDiff.finite_difference_jacobian(x -> get_t(x, b), a) - -# dget_tdb(a, b) -# FiniteDiff.finite_difference_jacobian(x -> get_t(a, x), b) +end \ No newline at end of file diff --git a/src/contacts/collisions/sphere_halfspace.jl b/src/contacts/collisions/sphere_halfspace.jl index cc7dfdede..abce1590e 100644 --- a/src/contacts/collisions/sphere_halfspace.jl +++ b/src/contacts/collisions/sphere_halfspace.jl @@ -63,7 +63,7 @@ end function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::SphereHalfSpaceCollision, xp, qp, xc, qc) if relative == :parent if jacobian == :parent - return 1.0 * I(3) + return 1.0 * sI(3) elseif jacobian == :child return szeros(eltype(xp), 3, 3) end diff --git a/src/contacts/cone.jl b/src/contacts/cone.jl index 705742075..9ff934483 100644 --- a/src/contacts/cone.jl +++ b/src/contacts/cone.jl @@ -4,7 +4,7 @@ function cone_product(u::AbstractVector{T}, v::AbstractVector{T}) where {T} end function cone_product(u::SVector{N,T}, v::SVector{N,T}) where {N,T} - vcat(u' * v, u[1] * v[SVector{N-1}(2:end)] + v[1] * u[SVector{N-1}(2:end)]) + vcat(u' * v, u[1] * v[SUnitRange(2,end)] + v[1] * u[SUnitRange(2,end)]) end function cone_product_jacobian(u::SVector{3,T}) where T diff --git a/src/contacts/contact.jl b/src/contacts/contact.jl index ad6eacfd9..256f743fb 100644 --- a/src/contacts/contact.jl +++ b/src/contacts/contact.jl @@ -125,7 +125,7 @@ function impulse_map_jacobian(relative::Symbol, jacobian::Symbol, model::Contact end Qx = rotation_matrix(inv(q)) * skew(r) * Xx - Qx -= rotation_matrix(inv(q)) * skew(X * λ) * (∂contact_point∂x(relative, jacobian, model.collision, xp, qp, xc, qc) - (relative == jacobian ? 1.0 : 0.0) * I(3)) + Qx -= rotation_matrix(inv(q)) * skew(X * λ) * (∂contact_point∂x(relative, jacobian, model.collision, xp, qp, xc, qc) - (relative == jacobian ? 1.0 : 0.0) * sI(3)) Qq = rotation_matrix(inv(q)) * skew(r) * Xq Qq -= rotation_matrix(inv(q)) * skew(X * λ) * ∂contact_point∂q(relative, jacobian, model.collision, xp, qp, xc, qc) diff --git a/src/contacts/linear.jl b/src/contacts/linear.jl index a83b5756c..725dcca26 100644 --- a/src/contacts/linear.jl +++ b/src/contacts/linear.jl @@ -90,8 +90,8 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where sγ = contact.impulses_dual[2][1] ψ = contact.impulses[2][2] sψ = contact.impulses_dual[2][2] - β = contact.impulses[2][@SVector [3,4,5,6]] - sβ = contact.impulses_dual[2][@SVector [3,4,5,6]] + β = contact.impulses[2][SA[3;4;5;6]] + sβ = contact.impulses_dual[2][SA[3;4;5;6]] SVector{N½,T}( d - sγ, diff --git a/src/contacts/nonlinear.jl b/src/contacts/nonlinear.jl index a21b4f383..9388402b1 100644 --- a/src/contacts/nonlinear.jl +++ b/src/contacts/nonlinear.jl @@ -71,7 +71,7 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where SVector{N½,T}( d - s[1], model.friction_coefficient * γ[1] - γ[2], - (model.friction_parameterization * vt - s[@SVector [3,4]])...) + (model.friction_parameterization * vt - s[SA[3;4]])...) end function constraint_jacobian(contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} diff --git a/src/contacts/velocity.jl b/src/contacts/velocity.jl index 470e0ec4d..e497ae87f 100644 --- a/src/contacts/velocity.jl +++ b/src/contacts/velocity.jl @@ -12,7 +12,7 @@ function ∂contact_point_velocity∂q(model::Contact, x, q, v, ω, c) end function ∂contact_point_velocity∂v(model::Contact, x, q, v, ω, c) - return 1.0 * I(3) + return 1.0 * sI(3) end function ∂contact_point_velocity∂ω(model::Contact, x, q, v, ω, c) diff --git a/src/gradients/utilities.jl b/src/gradients/utilities.jl index b04a34508..1770a393b 100644 --- a/src/gradients/utilities.jl +++ b/src/gradients/utilities.jl @@ -25,7 +25,7 @@ function attitude_jacobian(data::AbstractVector, Nb::Int) for i = 1:Nb x2, v15, q2, ω15 = unpack_data(data[13 * (i-1) .+ (1:13)]) q2 = Quaternion(q2...) - G = cat(G, I(6), LVᵀmat(q2), I(3), dims = (1,2)) + G = cat(G, sI(6), LVᵀmat(q2), sI(3), dims = (1,2)) end ndata = length(data) nu = ndata - size(G)[1] @@ -34,10 +34,10 @@ function attitude_jacobian(data::AbstractVector, Nb::Int) end function unpack_data(data::AbstractVector) - x2 = data[SVector{3,Int}(1:3)] - v15 = data[SVector{3,Int}(4:6)] - q2 = data[SVector{4,Int}(7:10)] - ω15 = data[SVector{3,Int}(11:13)] + x2 = data[SA[1;2;3]] + v15 = data[SA[4;5;6]] + q2 = data[SA[7;8;9;10]] + ω15 = data[SA[11;12;13]] return x2, v15, q2, ω15 end diff --git a/src/joints/constraints.jl b/src/joints/constraints.jl index d85ea6c0d..9d0c29482 100644 --- a/src/joints/constraints.jl +++ b/src/joints/constraints.jl @@ -411,7 +411,7 @@ function get_joint_impulses(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,N end n2 = n1 - 1 + impulses_length((joint.translational, joint.rotational)[i]) - λi = SVector{n2-n1+1,T}(joint.impulses[2][n1:n2]) + λi = SVector{n2-n1+1,T}(joint.impulses[2][SUnitRange(n1,n2)]) return λi end diff --git a/src/joints/joint.jl b/src/joints/joint.jl index ec7a7cb87..847d953d2 100644 --- a/src/joints/joint.jl +++ b/src/joints/joint.jl @@ -134,8 +134,8 @@ limits_length(joint::Joint{T,Nλ,Nb}) where {T,Nλ,Nb} = Nb impulses_length(joint::Joint{T,Nλ,Nb,N}) where {T,Nλ,Nb,N} = N function split_impulses(joint::Joint{T,Nλ,Nb}, η) where {T,Nλ,Nb} - s = η[SVector{Nb,Int}(1:Nb)] - γ = η[SVector{Nb,Int}(Nb .+ (1:Nb))] + s = η[SUnitRange(1,Nb)] + γ = η[SUnitRange(Nb+1,2*Nb)] return s, γ end diff --git a/src/joints/minimal.jl b/src/joints/minimal.jl index 78e3ee8d4..8594a7656 100644 --- a/src/joints/minimal.jl +++ b/src/joints/minimal.jl @@ -168,10 +168,10 @@ function set_minimal_coordinates_velocities!(joint::JointConstraint, Atra = zerodimstaticadjoint(nullspace_mask(tra)) # parent state - xa = SVector{3}(zp[1:3]) - va = SVector{3}(zp[3 .+ (1:3)]) - qa = Quaternion(zp[6 .+ (1:4)]...) - ωa = SVector{3}(zp[10 .+ (1:3)]) + xa = SVector{3}(zp[SA[1;2;3]]) + va = SVector{3}(zp[SA[4;5;6]]) + qa = Quaternion(zp[SA[7;8;9;10]]...) + ωa = SVector{3}(zp[SA[11;12;13]]) # positions Δq = axis_angle_to_quaternion(Arot * Δθ) @@ -236,7 +236,7 @@ function minimal_coordinates_velocities_jacobian_parent(joint::JointConstraint{T # Jacobians - ∂xb∂xa = 1.0 * I(3) + ∂xb∂xa = 1.0 * sI(3) ∂xb∂va = szeros(T, 3, 3) @@ -255,7 +255,7 @@ function minimal_coordinates_velocities_jacobian_parent(joint::JointConstraint{T ∂vb∂xa = szeros(T, 3, 3) - ∂vb∂va = 1.0 * I(3) + ∂vb∂va = 1.0 * sI(3) ∂vb∂qa = 1.0 / timestep * ∂vector_rotate∂q(pa + Atra * Δx, qa) ∂vb∂qa -= 1.0 / timestep * ∂vector_rotate∂q(pb, qb) * Rmat(orientation_offset * Δq) diff --git a/src/joints/rotational/dampers.jl b/src/joints/rotational/dampers.jl index 741b838db..d20c6d071 100644 --- a/src/joints/rotational/dampers.jl +++ b/src/joints/rotational/dampers.jl @@ -13,11 +13,11 @@ function damper_force(relative::Symbol, joint::Rotational{T}, qa::Quaternion, ω velocity = minimal_velocities(joint, szeros(3), szeros(3), qa, ωa, szeros(3), szeros(3), qb, ωb, timestep) if relative == :parent - force = 1.0000 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions + force = 1.0 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions rotate && (force = vector_rotate(force, orientation_offset)) # rotate back to frame a return [szeros(T, 3); force] elseif relative == :child - force = - 1.0000 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions + force = - 1.0 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions rotate && (force = vector_rotate(force, inv(qb) * qa * orientation_offset)) # rotate back to frame b return [szeros(T, 3); force] end diff --git a/src/joints/rotational/impulses.jl b/src/joints/rotational/impulses.jl index d0de5045d..96e3e6215 100644 --- a/src/joints/rotational/impulses.jl +++ b/src/joints/rotational/impulses.jl @@ -16,24 +16,24 @@ function impulse_transform_jacobian(relative::Symbol, jacobian::Symbol, # ∂(Ja'*p)/∂(xa,qa) Z3 = szeros(T,3,3) ∂Q∂qa = ∂VLᵀmat∂q(Tmat() * Rᵀmat(qb) * LVᵀmat(joint.orientation_offset) * p) * LVᵀmat(qa) - return cat(I(3), 0.5 * I(3), dims=(1,2)) * [Z3 Z3; Z3 ∂Q∂qa] + return cat(sI(3), 0.5 * sI(3), dims=(1,2)) * [Z3 Z3; Z3 ∂Q∂qa] elseif jacobian == :child # ∂(Ja'*p)/∂(xb,qb) Z3 = szeros(T,3,3) ∇Qqb = VLᵀmat(qa) * Tmat(T) * ∂Rᵀmat∂q(LVᵀmat(joint.orientation_offset) * p) * LVᵀmat(qb) - return cat(I(3), 0.5 * I(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqb] + return cat(sI(3), 0.5 * sI(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqb] end elseif relative == :child if jacobian == :parent # ∂(Jb'*p)/∂(xa,qa) Z3 = szeros(T,3,3) ∇Qqa = VLᵀmat(qb) * ∂Lmat∂q(LVᵀmat(joint.orientation_offset) * p) * LVᵀmat(qa) - return cat(I(3), 0.5 * I(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqa] + return cat(sI(3), 0.5 * sI(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqa] elseif jacobian == :child # ∂(Jb'*p)/∂(xb,qb) Z3 = szeros(T,3,3) ∇Qqb = ∂VLᵀmat∂q(Lmat(qa) * LVᵀmat(joint.orientation_offset) * p) * LVᵀmat(qb) - return cat(I(3), 0.5 * I(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqb] + return cat(sI(3), 0.5 * sI(3), dims=(1,2)) * [Z3 Z3; Z3 ∇Qqb] end end end \ No newline at end of file diff --git a/src/joints/rotational/springs.jl b/src/joints/rotational/springs.jl index 564ea5960..a555df4b2 100644 --- a/src/joints/rotational/springs.jl +++ b/src/joints/rotational/springs.jl @@ -64,11 +64,11 @@ function spring_jacobian_configuration(relative::Symbol, jacobian::Symbol, joint J1 = timestep * rotation_matrix(inv(qb) * qa * joint.orientation_offset) * -spring * zerodimstaticadjoint(nullspace_mask(joint)) * minimal_coordinates_jacobian_configuration(jacobian, joint, xa, qa, xb, qb, attjac=attjac) if jacobian == :parent - Q2 = timestep * ∂vector_rotate∂q(force[SVector{3,Int}(4,5,6)], inv(qb) * qa * joint.orientation_offset) * Rmat(joint.orientation_offset) * Lmat(inv(qb)) + Q2 = timestep * ∂vector_rotate∂q(force[SA[4;5;6]], inv(qb) * qa * joint.orientation_offset) * Rmat(joint.orientation_offset) * Lmat(inv(qb)) attjac && (Q2 *= LVᵀmat(qa)) J2 = [X Q2] elseif jacobian == :child - Q2 = timestep * ∂vector_rotate∂q(force[SVector{3,Int}(4,5,6)], inv(qb) * qa * joint.orientation_offset) * Rmat(qa * joint.orientation_offset) * Tmat() + Q2 = timestep * ∂vector_rotate∂q(force[SA[4;5;6]], inv(qb) * qa * joint.orientation_offset) * Rmat(qa * joint.orientation_offset) * Tmat() attjac && (Q2 *= LVᵀmat(qb)) J2 = [X Q2] end diff --git a/src/joints/translational/input.jl b/src/joints/translational/input.jl index a703b4771..7d7475e11 100644 --- a/src/joints/translational/input.jl +++ b/src/joints/translational/input.jl @@ -37,8 +37,8 @@ function input_jacobian_control(relative::Symbol, input_scaling) Ta = impulse_transform(relative, joint, xa, qa, xb, qb) - X = Ta[1:3,1:3] - Q = 0.5 * Ta[4:6,1:3] + X = Ta[SA[1;2;3],SA[1;2;3]] + Q = 0.5 * Ta[SA[4;5;6],SA[1;2;3]] return [X; Q] * input_scaling end @@ -49,17 +49,17 @@ function input_jacobian_configuration(relative::Symbol, # d[Faw;2τaa]/d[xa,qa] ∇aa = impulse_transform_jacobian(:parent, relative, joint, xa, qa, xb, qb, joint.input) - FaXa = ∇aa[1:3, 1:3] - FaQa = ∇aa[1:3, 4:6] - τaXa = 0.5 * ∇aa[4:6, 1:3] - τaQa = 0.5 * ∇aa[4:6, 4:6] + FaXa = ∇aa[SA[1;2;3], SA[1;2;3]] + FaQa = ∇aa[SA[1;2;3], SA[4;5;6]] + τaXa = 0.5 * ∇aa[SA[4;5;6], SA[1;2;3]] + τaQa = 0.5 * ∇aa[SA[4;5;6], SA[4;5;6]] # d[Fbw;2τbb]/d[xa,qa] ∇ba = impulse_transform_jacobian(:child, relative, joint, xa, qa, xb, qb, joint.input) - FbXa = ∇ba[1:3,1:3] - FbQa = ∇ba[1:3,4:6] - τbXa = 0.5 * ∇ba[4:6,1:3] - τbQa = 0.5 * ∇ba[4:6,4:6] + FbXa = ∇ba[SA[1;2;3],SA[1;2;3]] + FbQa = ∇ba[SA[1;2;3],SA[4;5;6]] + τbXa = 0.5 * ∇ba[SA[4;5;6],SA[1;2;3]] + τbQa = 0.5 * ∇ba[SA[4;5;6],SA[4;5;6]] return FaXa, FaQa, τaXa, τaQa, FbXa, FbQa, τbXa, τbQa end diff --git a/src/joints/translational/springs.jl b/src/joints/translational/springs.jl index 32f78ac2d..15b054915 100644 --- a/src/joints/translational/springs.jl +++ b/src/joints/translational/springs.jl @@ -25,7 +25,7 @@ function spring_impulses(relative::Symbol, joint::Translational, xa::AbstractVector, qa::Quaternion, xb::AbstractVector, qb::Quaternion, timestep; unitary::Bool=false) - timestep * impulse_transform(relative, joint, xa, qa, xb, qb) * spring_force(relative, joint, xa, qa, xb, qb; unitary=unitary)[SVector{3,Int}(1,2,3)] + timestep * impulse_transform(relative, joint, xa, qa, xb, qb) * spring_force(relative, joint, xa, qa, xb, qb; unitary=unitary)[SA[1;2;3]] end spring_impulses(relative::Symbol, joint::Translational{T,3}, pbody::Node, cbody::Node, timestep; unitary::Bool=false) where T = szeros(T, 6) @@ -52,7 +52,7 @@ function spring_jacobian_configuration(relative::Symbol, jacobian_relative::Symb unitary::Bool=false, attjac=true) where T - force = spring_force(relative, joint, xa, qa, xb, qb, unitary=unitary)[SVector{3,Int}(1,2,3)] + force = spring_force(relative, joint, xa, qa, xb, qb, unitary=unitary)[SA[1;2;3]] J = impulse_transform(relative, joint, xa, qa, xb, qb) * spring_force_jacobian_configuration(jacobian_relative, joint, xa, qa, xb, qb, unitary=unitary, attjac=attjac) diff --git a/src/mechanism/data.jl b/src/mechanism/data.jl index 5921544ba..00fcfaa89 100644 --- a/src/mechanism/data.jl +++ b/src/mechanism/data.jl @@ -100,15 +100,15 @@ function set_data!(mechanism::Mechanism, data::AbstractVector) c = 0 for joint in mechanism.joints Nd = data_dim(joint) - set_data!(joint, data[c .+ (1:Nd)]); c += Nd + set_data!(joint, data[SUnitRange(c+1,c+Nd)]); c += Nd end for body in mechanism.bodies Nd = data_dim(body, attjac=false) - set_data!(body, data[c .+ (1:Nd)], mechanism.timestep); c += Nd + set_data!(body, data[SUnitRange(c+1,c+Nd)], mechanism.timestep); c += Nd end for contact in mechanism.contacts Nd = data_dim(contact) - set_data!(contact, data[c .+ (1:Nd)]); c += Nd + set_data!(contact, data[SUnitRange(c+1,c+Nd)]); c += Nd end for joint in mechanism.joints input_impulse!(joint, mechanism, false) @@ -160,27 +160,27 @@ end function set_data!(model::NonlinearContact, data::AbstractVector) model.friction_coefficient = data[1] model.collision.contact_radius = data[2] - model.collision.contact_origin = data[SVector{3,Int}(3:5)] + model.collision.contact_origin = data[SA[3;4;5]] return nothing end function set_data!(model::LinearContact, data::AbstractVector) model.friction_coefficient = data[1] model.collision.contact_radius = data[2] - model.collision.contact_origin = data[SVector{3,Int}(3:5)] + model.collision.contact_origin = data[SA[3;4;5]] return nothing end function set_data!(model::ImpactContact, data::AbstractVector) model.collision.contact_radius = data[1] - model.collision.contact_origin = data[SVector{3,Int}(2:4)] + model.collision.contact_origin = data[SA[2;3;4]] return nothing end function set_data!(contact::ContactConstraint, data::AbstractVector) model = contact.model N = data_dim(model) - set_data!(model, data[1:N]) + set_data!(model, data[SUnitRange(1,N)]) return nothing end @@ -188,7 +188,7 @@ function set_data!(contacts::Vector{<:ContactConstraint}, data::AbstractVector) c = 0 for contact in contacts Nd = data_dim(contact) - set_data!(contact, data[c .+ (1:Nd)]) + set_data!(contact, data[SUnitRange(c+1,c+Nd)]) c += Nd end return nothing diff --git a/src/mechanism/set.jl b/src/mechanism/set.jl index efcecfe1e..e960aa9a6 100644 --- a/src/mechanism/set.jl +++ b/src/mechanism/set.jl @@ -10,7 +10,7 @@ function set_maximal_state!(mechanism::Mechanism, z::AbstractVector) off = 0 for body in mechanism.bodies - x2, v15, q2, ω15 = unpack_data(z[off+1:end]); off += 13 + x2, v15, q2, ω15 = unpack_data(z[SUnitRange(off+1,end)]); off += 13 q2 = Quaternion(q2...) body.state.v15 = v15 body.state.ω15 = ω15 @@ -43,7 +43,7 @@ function set_input!(mechanism::Mechanism{T}, u::AbstractVector) where T off = 0 for joint in joints nu = input_dimension(joint) - set_input!(joint, SVector{nu,T}(u[off .+ (1:nu)])) + set_input!(joint, SVector{nu,T}(u[SUnitRange(off+1,off+nu)])) off += nu end # apply the controls to each body's state @@ -130,7 +130,7 @@ function set_spring_offset!(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, x::AbstractVect off = 0 for joint in mechanism.joints N̄ = 3 - length(joint) - joint.spring_offset = x[off .+ (1:N̄)] + joint.spring_offset = x[SUnitRange(off+1,off+N̄)] off += 2N̄ end return diff --git a/src/mechanism/state.jl b/src/mechanism/state.jl index b348e247f..f4beb06bb 100644 --- a/src/mechanism/state.jl +++ b/src/mechanism/state.jl @@ -14,8 +14,8 @@ function minimal_to_maximal(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, y::AbstractVect (id > Ne) && continue # only treat joints joint = temp_mechanism.joints[id] nu = input_dimension(joint) - off = sum(nus[1:id-1])*2 - set_minimal_coordinates_velocities!(temp_mechanism, joint, xmin=y[off .+ SUnitRange(1, 2nu)]) # TODO does this actually set a state and not just convert min to max? + off = sum(nus[SUnitRange(1,id-1)])*2 + set_minimal_coordinates_velocities!(temp_mechanism, joint, xmin=y[SUnitRange(off+1,off+2nu)]) # TODO does this actually set a state and not just convert min to max? end return get_maximal_state(temp_mechanism) @@ -66,7 +66,7 @@ function maximal_to_minimal(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, z::AbstractVect end function unpack_maximal_state(z::AbstractVector, i::Int) - zi = z[(i-1) * 13 .+ (1:13)] + zi = z[SUnitRange((i-1)*13+1,i*13)] x2 = zi[SUnitRange(1,3)] v15 = zi[SUnitRange(4,6)] q2 = Quaternion(zi[7:10]...) @@ -78,10 +78,10 @@ function pack_maximal_state!(z::AbstractVector, x2::AbstractVector, v15::AbstractVector, q2::Quaternion, ω15::AbstractVector, i::Int) - z[(i-1) * 13 .+ (1:3)] = x2 - z[(i-1) * 13 .+ (4:6)] = v15 - z[(i-1) * 13 .+ (7:10)] = vector(q2) - z[(i-1) * 13 .+ (11:13)] = ω15 + z[SUnitRange((i-1)*13+1,(i-1)*13+3)] = x2 + z[SUnitRange((i-1)*13+4,(i-1)*13+6)] = v15 + z[SUnitRange((i-1)*13+7,(i-1)*13+10)] = vector(q2) + z[SUnitRange((i-1)*13+11,(i-1)*13+13)] = ω15 return nothing end diff --git a/src/mechanism/urdf.jl b/src/mechanism/urdf.jl index 10e69b214..d7f5dbfe5 100644 --- a/src/mechanism/urdf.jl +++ b/src/mechanism/urdf.jl @@ -177,7 +177,7 @@ function parse_link(xlink, materialdict, T; path_prefix) shape = nothing elseif length(shapes) > 1 s = [s.shape for s in shapes] - shape = CombinedShapes(s, 0.0, diagm([0.0; 0.0; 0.0])) + shape = CombinedShapes(s, 0.0, szeros(3)) else shape = shapes[1] end diff --git a/src/orientation/mrp.jl b/src/orientation/mrp.jl index ffbe71200..f8cd5de6c 100644 --- a/src/orientation/mrp.jl +++ b/src/orientation/mrp.jl @@ -9,7 +9,7 @@ end function dmrpdq(q::AbstractVector) s = q[1] - v = q[2:4] + v = q[SA[2;3;4]] d1 = 1 / (s + 1)^2 di = 1 / (s + 1) diff --git a/src/orientation/rotate.jl b/src/orientation/rotate.jl index c9cd1d42a..db085c770 100644 --- a/src/orientation/rotate.jl +++ b/src/orientation/rotate.jl @@ -7,9 +7,9 @@ vector_rotate(v::AbstractVector,q::Quaternion) = Vmat(quaternion_rotate(Quaterni # rotate matrix function matrix_rotate(A::AbstractMatrix,q::Quaternion) - c1 = vector_rotate(A[SVector{3}(1,2,3)], q) - c2 = vector_rotate(A[SVector{3}(4,5,6)], q) - c3 = vector_rotate(A[SVector{3}(7,8,9)], q) + c1 = vector_rotate(A[SA[1;2;3]], q) + c2 = vector_rotate(A[SA[4;5;6]], q) + c3 = vector_rotate(A[SA[7;8;9]], q) return [c1 c2 c3] end diff --git a/src/simulation/storage.jl b/src/simulation/storage.jl index 64f4594ff..5541dddba 100644 --- a/src/simulation/storage.jl +++ b/src/simulation/storage.jl @@ -74,10 +74,10 @@ function generate_storage(mechanism::Mechanism, z) for t = 1:N off = 0 for i = 1:M - storage.x[i][t] = z[t][off .+ (1:3)] - storage.v[i][t] = z[t][off .+ (4:6)] - storage.q[i][t] = Quaternion(z[t][off .+ (7:10)]...) - storage.ω[i][t] = z[t][off .+ (11:13)] + storage.x[i][t] = z[t][SUnitRange(off+1,off+3)] + storage.v[i][t] = z[t][SUnitRange(off+4,off+6)] + storage.q[i][t] = Quaternion(z[t][SUnitRange(off+7,off+10)]...) + storage.ω[i][t] = z[t][SUnitRange(off+11,off+13)] off += 13 end end @@ -98,7 +98,7 @@ function get_maximal_state(storage::Storage{T,N}, i::Int) where {T,N} q2 = storage.q[j][i] v15 = storage.v[j][i] ω15 = storage.ω[j][i] - z[13 * (j-1) .+ (1:13)] = [x2; v15; vector(q2); ω15] + z[SUnitRange((j-1)*13+1,j*13)] = [x2; v15; vector(q2); ω15] end return z end diff --git a/src/solver/centering.jl b/src/solver/centering.jl index 012bf5698..a4683dbb7 100644 --- a/src/solver/centering.jl +++ b/src/solver/centering.jl @@ -1,40 +1,49 @@ function centering!(mechanism::Mechanism, αaff::T) where T system = mechanism.system - n = 0 - ν = 0.0 - νaff = 0.0 + parameters = [SA{T}[0;0;0]] # ν, νaff, n for contact in mechanism.contacts - ν, νaff, n = centering!(ν, νaff, n, mechanism, contact, get_entry(system, contact.id), αaff) + centering!(parameters, contact, get_entry(system, contact.id), αaff) end for joint in mechanism.joints - ν, νaff, n = centering!(ν, νaff, n, mechanism, joint, get_entry(system, joint.id), αaff) + centering!(parameters, joint, get_entry(system, joint.id), αaff) end - ν /= n - νaff /= n + ν = parameters[1][1]/parameters[1][3] + νaff = parameters[1][2]/parameters[1][3] return ν, νaff end -function centering!(ν, νaff, n, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, vector_entry::Entry, αaff) where {T,N,Nc,Cs,N½} +function centering!(parameters, contact::ContactConstraint{T,N,Nc,Cs,N½}, vector_entry::Entry, αaff) where {T,N,Nc,Cs,N½} s = contact.impulses_dual[2] γ = contact.impulses[2] - Δs = vector_entry.value[1:N½] - Δγ = vector_entry.value[N½ .+ (1:N½)] - ν += dot(s, γ) - νaff += dot(s + αaff * Δs, γ + αaff * Δγ) # plus or minus - n += cone_degree(contact) - return ν, νaff, n -end + Δs = vector_entry.value[SUnitRange(1,N½)] + Δγ = vector_entry.value[SUnitRange(N½+1,N)] + ν = dot(s, γ) + νaff = dot(s + αaff * Δs, γ + αaff * Δγ) # plus or minus + n = cone_degree(contact) + parameters[1] += SA{T}[ν;νaff;n] -function centering!(ν, νaff, n, mechanism, joint::JointConstraint{T,N,Nc}, vector_entry::Entry, αaff) where {T,N,Nc} - for (i, element) in enumerate((joint.translational, joint.rotational)) - s, γ = split_impulses(element, joint.impulses[2][joint_impulse_index(joint,i)]) - Δs, Δγ = split_impulses(element, vector_entry.value[joint_impulse_index(joint,i)]) - ν += dot(s, γ) - νaff += dot(s + αaff * Δs, γ + αaff * Δγ) # plus or minus - n += length(s) - end - return ν, νaff, n + return end + +function centering!(parameters, joint::JointConstraint{T,N,Nc}, vector_entry::Entry, αaff) where {T,N,Nc} + joint_t = joint.translational + s, γ = split_impulses(joint_t, joint.impulses[2][joint_impulse_index(joint,1)]) + Δs, Δγ = split_impulses(joint_t, vector_entry.value[joint_impulse_index(joint,1)]) + ν = dot(s, γ) + νaff = dot(s + αaff * Δs, γ + αaff * Δγ) # plus or minus + n = length(s) + parameters[1] += SA{T}[ν;νaff;n] + + joint_r = joint.rotational + s, γ = split_impulses(joint_r, joint.impulses[2][joint_impulse_index(joint,2)]) + Δs, Δγ = split_impulses(joint_r, vector_entry.value[joint_impulse_index(joint,2)]) + ν = dot(s, γ) + νaff = dot(s + αaff * Δs, γ + αaff * Δγ) # plus or minus + n = length(s) + parameters[1] += SA{T}[ν;νaff;n] + + return +end \ No newline at end of file diff --git a/src/solver/complementarity.jl b/src/solver/complementarity.jl index 158212861..3276f7879 100644 --- a/src/solver/complementarity.jl +++ b/src/solver/complementarity.jl @@ -18,7 +18,7 @@ complementarity(mechanism, contact::ContactConstraint) = contact.impulses[2] .* function complementarity(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs<:NonlinearContact{T,N}} γ = contact.impulses[2] s = contact.impulses_dual[2] - return vcat(γ[1] * s[1], cone_product(γ[@SVector [2,3,4]], s[@SVector [2,3,4]])) + return vcat(γ[1] * s[1], cone_product(γ[SA[2,3,4]], s[SA[2,3,4]])) end complementarityμ(mechanism, contact::ContactConstraint) = complementarity(mechanism, contact) - mechanism.μ * neutral_vector(contact.model) diff --git a/src/solver/correction.jl b/src/solver/correction.jl index ad748f6e5..24b173484 100644 --- a/src/solver/correction.jl +++ b/src/solver/correction.jl @@ -11,8 +11,8 @@ end correction!(mechanism::Mechanism, residual_entry::Entry, step_entry::Entry, node::Node) = nothing function correction!(mechanism::Mechanism, residual_entry::Entry, step_entry::Entry, ::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs,N½} - Δs = step_entry.value[1:N½] - Δγ = step_entry.value[N½ .+ (1:N½)] + Δs = step_entry.value[SUnitRange(1,N½)] + Δγ = step_entry.value[SUnitRange(N½+1,N)] μ = mechanism.μ residual_entry.value += [- Δs .* Δγ .+ μ; szeros(N½)] return @@ -21,9 +21,9 @@ end function correction!(mechanism::Mechanism, residual_entry::Entry, step_entry::Entry, contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½} cont = contact.model μ = mechanism.μ - Δs = step_entry.value[1:N½] - Δγ = step_entry.value[N½ .+ (1:N½)] - residual_entry.value += [[-Δs[1] * Δγ[1]; -cone_product(Δs[2:4], Δγ[2:4])] + μ * neutral_vector(cont); szeros(N½)] + Δs = step_entry.value[SUnitRange(1,N½)] + Δγ = step_entry.value[SUnitRange(N½+1,N)] + residual_entry.value += [[-Δs[1] * Δγ[1]; -cone_product(Δs[SA[2;3;4]], Δγ[SA[2;3;4]])] + μ * neutral_vector(cont); szeros(N½)] return end diff --git a/src/solver/initialization.jl b/src/solver/initialization.jl index 6a1a35346..daf9cc823 100644 --- a/src/solver/initialization.jl +++ b/src/solver/initialization.jl @@ -5,12 +5,12 @@ function initialize!(contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs} end function initialize!(contact::ContactConstraint{T,N,Nc,Cs}) where {T,N,Nc,Cs<:NonlinearContact{T,N}} - γort, sort = initialize_positive_orthant!(contact.impulses[1][1:1], contact.impulses_dual[1][1:1]) - γsoc, ssoc = initialize_second_order_cone!(contact.impulses[1][2:4], contact.impulses_dual[1][2:4]) + γort, sort = initialize_positive_orthant!(contact.impulses[1][SA[1]], contact.impulses_dual[1][SA[1]]) + γsoc, ssoc = initialize_second_order_cone!(contact.impulses[1][SA[2;3;4]], contact.impulses_dual[1][SA[2;3;4]]) contact.impulses[1] = [γort; γsoc] contact.impulses_dual[1] = [sort; ssoc] - γort, sort = initialize_positive_orthant!(contact.impulses[2][1:1], contact.impulses_dual[2][1:1]) - γsoc, ssoc = initialize_second_order_cone!(contact.impulses[2][2:4], contact.impulses_dual[2][2:4]) + γort, sort = initialize_positive_orthant!(contact.impulses[2][SA[1]], contact.impulses_dual[2][SA[1]]) + γsoc, ssoc = initialize_second_order_cone!(contact.impulses[2][SA[2;3;4]], contact.impulses_dual[2][SA[2;3;4]]) contact.impulses[2] = [γort; γsoc] contact.impulses_dual[2] = [sort; ssoc] return nothing @@ -34,14 +34,14 @@ end function initialize_second_order_cone!(γ::AbstractVector{T}, s::AbstractVector{T}; ϵ::T = 1e-20) where T - e = [1.0; zeros(length(γ) - 1)] # identity element - δs = max(-1.5 * (s[1] - norm(s[2:end])), 0) - δγ = max(-1.5 * (γ[1] - norm(γ[2:end])), 0) + e = [1.0; szeros(length(γ) - 1)] # identity element + δs = max(-1.5 * (s[1] - norm(s[SUnitRange(2,end)])), 0) + δγ = max(-1.5 * (γ[1] - norm(γ[SUnitRange(2,end)])), 0) sh = s + δs * e γh = γ + δγ * e - δhs = 0.5 * transpose(sh) * γh / ((γh[1] + norm(γh[2,end])) + ϵ) - δhγ = 0.5 * transpose(sh) * γh / ((sh[1] + norm(sh[2,end])) + ϵ) + δhs = 0.5 * transpose(sh) * γh / ((γh[1] + norm(γh[SUnitRange(2,end)])) + ϵ) + δhγ = 0.5 * transpose(sh) * γh / ((sh[1] + norm(sh[SUnitRange(2,end)])) + ϵ) s0 = sh + δhs * e γ0 = γh + δhγ * e diff --git a/src/solver/line_search.jl b/src/solver/line_search.jl index d18dc816b..a7c8341d7 100644 --- a/src/solver/line_search.jl +++ b/src/solver/line_search.jl @@ -55,12 +55,12 @@ function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N s = contact.impulses_dual[2] γ = contact.impulses[2] - Δs = vector_entry.value[1:N½] - Δγ = vector_entry.value[N½ .+ (1:N½)] - αs_ort = positive_orthant_step_length(s[1:1], Δs[1:1], τ = τort) - αγ_ort = positive_orthant_step_length(γ[1:1], Δγ[1:1], τ = τort) - αs_soc = second_order_cone_step_length(s[2:4], Δs[2:4]; τ = τsoc) - αγ_soc = second_order_cone_step_length(γ[2:4], Δγ[2:4]; τ = τsoc) + Δs = vector_entry.value[SUnitRange(1,N½)] + Δγ = vector_entry.value[SUnitRange(N½+1,2*N½)] + αs_ort = positive_orthant_step_length(s[SA[1]], Δs[SA[1]], τ = τort) + αγ_ort = positive_orthant_step_length(γ[SA[1]], Δγ[SA[1]], τ = τort) + αs_soc = second_order_cone_step_length(s[SA[2;3;4]], Δs[SA[2;3;4]]; τ = τsoc) + αγ_soc = second_order_cone_step_length(γ[SA[2;3;4]], Δγ[SA[2;3;4]]; τ = τsoc) return min(α, αs_soc, αγ_soc, αs_ort, αγ_ort) end @@ -70,8 +70,8 @@ function cone_line_search!(α, mechanism, contact::ContactConstraint{T,N,Nc,Cs,N s = contact.impulses_dual[2] γ = contact.impulses[2] - Δs = vector_entry.value[1:N½] - Δγ = vector_entry.value[N½ .+ (1:N½)] + Δs = vector_entry.value[SUnitRange(1,N½)] + Δγ = vector_entry.value[SUnitRange(N½+1,2*N½)] αs_ort = positive_orthant_step_length(s, Δs, τ = τort) @@ -118,17 +118,18 @@ function second_order_cone_step_length(λ::AbstractVector{T}, Δ::AbstractVector # check Section 8.2 CVXOPT λ0 = λ[1] - λ_λ = max(λ0^2 - λ[2:end]' * λ[2:end], 1e-25) + λ_2_end = λ[SUnitRange(2,end)] + λ_λ = max(λ0^2 - λ_2_end' * λ_2_end, 1e-25) if λ_λ < 0.0 # @show λ_λ @warn "should always be positive" end λ_λ += ϵ - λ_Δ = λ0 * Δ[1] - λ[2:end]' * Δ[2:end] + ϵ + λ_Δ = λ0 * Δ[1] - λ_2_end' * Δ[SUnitRange(2,end)] + ϵ ρs = λ_Δ / λ_λ - ρv = Δ[2:end] / sqrt(λ_λ) - ρv -= (λ_Δ / sqrt(λ_λ) + Δ[1]) / (λ0 / sqrt(λ_λ) + 1) * λ[2:end] / λ_λ + ρv = Δ[SUnitRange(2,end)] / sqrt(λ_λ) + ρv -= (λ_Δ / sqrt(λ_λ) + Δ[1]) / (λ0 / sqrt(λ_λ) + 1) * λ_2_end / λ_λ α = 1.0 if norm(ρv) - ρs > 0.0 α = min(α, τ / (norm(ρv) - ρs)) diff --git a/src/solver/violations.jl b/src/solver/violations.jl index ca3ea5d00..f5e0f5165 100644 --- a/src/solver/violations.jl +++ b/src/solver/violations.jl @@ -8,41 +8,58 @@ function residual_violation(mechanism::Mechanism) violation = max(violation, norm(res, Inf)) end for contact in mechanism.contacts - res = constraint(mechanism, contact) - violation = max(violation, norm(res, Inf)) + violation = max(violation, contact_residual_violation(mechanism, contact)) end return violation end -# TODO probably could be made alloc free with @generated or dispatching function joint_residual_violation(mechanism, joint) res = constraint(mechanism, joint) joint_t = joint.translational Nλ = joint_length(joint_t) Nb = limits_length(joint_t) - subres = res[2Nb .+ (1:Nλ)] + subres = res[SUnitRange(2*Nb+1,2*Nb+Nλ)] violation = norm(subres, Inf) shift = impulses_length(joint_t) joint_r = joint.rotational Nλ = joint_length(joint_r) Nb = limits_length(joint_r) - subres = res[shift + 2Nb .+ (1:Nλ)] + subres = res[SUnitRange(shift+2Nb+1,shift+2Nb+Nλ)] violation = max(violation, norm(subres, Inf)) return violation end +function contact_residual_violation(mechanism, contact) + res = constraint(mechanism, contact) + violation = norm(res, Inf) + + return violation +end + function bilinear_violation(mechanism::Mechanism) violation = 0.0 - for contact in mechanism.contacts - comp = complementarity(mechanism, contact) - violation = max(violation, norm(comp, Inf)) - end for joint in mechanism.joints - comp = complementarity(mechanism, joint) - violation = max(violation, norm(comp, Inf)) + violation = max(violation, joint_bilinear_violation(mechanism, joint)) + end + for contact in mechanism.contacts + violation = max(violation, contact_bilinear_violation(mechanism, contact)) end return violation end + +function joint_bilinear_violation(mechanism, joint) + comp = complementarity(mechanism, joint) + violation = norm(comp, Inf) + + return violation +end + +function contact_bilinear_violation(mechanism, contact) + comp = complementarity(mechanism, contact) + violation = norm(comp, Inf) + + return violation +end \ No newline at end of file diff --git a/src/utilities/custom_static.jl b/src/utilities/custom_static.jl index 184238cfb..30121d239 100644 --- a/src/utilities/custom_static.jl +++ b/src/utilities/custom_static.jl @@ -18,6 +18,9 @@ sones(N)= @SVector ones(N) srand(N)= @SVector rand(N) +sI(::Type{T}, N) where T = SMatrix{3,3,T}(I) +sI(N) = SMatrix{3,3,Float64}(I) + # TODO: check StaticArray bug fix, then remove zerodimstaticadjoint(A) = A' zerodimstaticadjoint(::SMatrix{0,N,T,0}) where {T,N} = SMatrix{N,0,T,0}() diff --git a/src/visuals/rope.jl b/src/visuals/rope.jl index f13673f51..070e8cb30 100644 --- a/src/visuals/rope.jl +++ b/src/visuals/rope.jl @@ -51,7 +51,7 @@ end function link_transform(start, goal) # transforms a vertical line of length 1 into a line between start and goal v1 = [0.0, 0.0, 1.0] - v2 = goal[1:3] - start[1:3] + v2 = goal[SA[1;2;3]] - start[SA[1;2;3]] normalize!(v2) ax = cross(v1, v2) ang = acos(v1' * v2) @@ -95,12 +95,12 @@ end function set_loose_rope(vis::Visualizer, x_start, x_goal; N::Int=10, rope_length=2norm(x_goal - x_start), min_altitude=-Inf, a_guess=1.0, dx_guess=0.0, name::Symbol=:rope) v = x_goal - x_start - shadow_rope_length = norm(v[1:2]) + shadow_rope_length = norm(v[SA[1;2]]) θ = atan(v[2], v[1]) R = rotationmatrix(RotZ(-θ)) v̄ = R * v # rotated into the xz plane - a, dx, dy = catenary_parameters(zeros(2), v̄[[1,3]], rope_length, a_guess=a_guess, dx_guess=dx_guess) + a, dx, dy = catenary_parameters(zeros(2), v̄[SA[1;3]], rope_length, a_guess=a_guess, dx_guess=dx_guess) Λ = shadow_rope_length * range(0,1,length=N+1) x = [] for i = 1:N+1 From e763ce17398f4b0f31f570c4e05b94b6cfd8afde Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Thu, 13 Apr 2023 17:43:37 +0200 Subject: [PATCH 5/6] Fix sI --- docs/src/contributing.md | 2 +- src/mechanism/state.jl | 2 +- src/mechanism/urdf.jl | 2 +- src/utilities/custom_static.jl | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/src/contributing.md b/docs/src/contributing.md index f2720db7b..a16f5d321 100644 --- a/docs/src/contributing.md +++ b/docs/src/contributing.md @@ -9,7 +9,7 @@ Contributions are always welcome! ## Potentially Useful Contributions Here are a list of current to-do's that would make awesome contributions: -- reduce allocations by using StaticArrays in functions +- reduce allocations by using StaticArrays and https://docs.julialang.org/en/v1/manual/profile/#Line-by-Line-Allocation-Tracking - improved parsing of URDF files - joint limits, friction coefficients - improved collision detection diff --git a/src/mechanism/state.jl b/src/mechanism/state.jl index f4beb06bb..d4f43a0f9 100644 --- a/src/mechanism/state.jl +++ b/src/mechanism/state.jl @@ -69,7 +69,7 @@ function unpack_maximal_state(z::AbstractVector, i::Int) zi = z[SUnitRange((i-1)*13+1,i*13)] x2 = zi[SUnitRange(1,3)] v15 = zi[SUnitRange(4,6)] - q2 = Quaternion(zi[7:10]...) + q2 = Quaternion(zi[SUnitRange(7,10)]...) ω15 = zi[SUnitRange(11,13)] return x2, v15, q2, ω15 end diff --git a/src/mechanism/urdf.jl b/src/mechanism/urdf.jl index d7f5dbfe5..ce52902b0 100644 --- a/src/mechanism/urdf.jl +++ b/src/mechanism/urdf.jl @@ -177,7 +177,7 @@ function parse_link(xlink, materialdict, T; path_prefix) shape = nothing elseif length(shapes) > 1 s = [s.shape for s in shapes] - shape = CombinedShapes(s, 0.0, szeros(3)) + shape = CombinedShapes(s, 0.0, szeros(3,3)) else shape = shapes[1] end diff --git a/src/utilities/custom_static.jl b/src/utilities/custom_static.jl index 30121d239..22eb52c59 100644 --- a/src/utilities/custom_static.jl +++ b/src/utilities/custom_static.jl @@ -18,8 +18,8 @@ sones(N)= @SVector ones(N) srand(N)= @SVector rand(N) -sI(::Type{T}, N) where T = SMatrix{3,3,T}(I) -sI(N) = SMatrix{3,3,Float64}(I) +sI(::Type{T}, N) where T = SMatrix{N,N,T}(I) +sI(N) = SMatrix{N,N,Float64}(I) # TODO: check StaticArray bug fix, then remove zerodimstaticadjoint(A) = A' From 43b0a2898a0ad487184bf6be95d08a7b4aab3a00 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Thu, 13 Apr 2023 17:49:51 +0200 Subject: [PATCH 6/6] Fix utilities test --- test/utilities.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/test/utilities.jl b/test/utilities.jl index 089a1b881..5bf22ecec 100644 --- a/test/utilities.jl +++ b/test/utilities.jl @@ -25,7 +25,6 @@ display(test_joint.rotational) test_mechanism = Mechanism(test_origin,[test_body],[test_joint]) display(test_mechanism) -display(test_mechanism.matrix_entries[1]) # display(test_mechanism.system) # TODO Removed from tests because '_show_with_braille_patterns' fails during tests for Julia 1.6, but it works in GBS for 1.6. @test true