From 1d0a8a0f357878fe18975bfa196932345009e2c2 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Fri, 27 Nov 2020 12:52:28 +0100 Subject: [PATCH 1/5] Add joint cat --- src/main_components/equalityconstraint.jl | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/main_components/equalityconstraint.jl b/src/main_components/equalityconstraint.jl index a5b877af..3245817d 100644 --- a/src/main_components/equalityconstraint.jl +++ b/src/main_components/equalityconstraint.jl @@ -192,4 +192,23 @@ end function ∂g∂posbc(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc} vec = [hcat(∂g∂posbc(eqc.constraints[i], getbody(mechanism, eqc.parentid), getbody(mechanism, id))) for i = 1:Nc] return vcat(vec...) +end + +function Base.cat(eqc1::EqualityConstraint{T,N1,Nc1}, eqc2::EqualityConstraint{T,N2,Nc2}) where {T,N1,N2,Nc1,Nc2} + @assert eqc1.parentid == eqc2.parentid "Can only concatenate constraints with the same parentid" + parentid = eqc1.parentid + if parentid === nothing + parentid = -1 + nothingflag = true + else + nothingflag = false + end + + constraints = [[eqc1.constraints[i] for i=1:Nc1];[eqc2.constraints[i] for i=1:Nc2]] + childids = [[eqc1.childids[i] for i=1:Nc1];[eqc2.childids[i] for i=1:Nc2]] + + eqc = EqualityConstraint([(constraints[i],parentid,childids[i]) for i=1:Nc1+Nc2]..., name="combined_"*eqc1.name*eqc2.name) + nothingflag && (eqc.parentid = nothing) + + return eqc end \ No newline at end of file From 4f89cf0dd5b350dd5536f5d288b93ba8067f8051 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Tue, 1 Dec 2020 15:25:53 +0100 Subject: [PATCH 2/5] Initial urdf loop parsing --- benchmark/example_benchmark.jl | 2 +- src/main_components/equalityconstraint.jl | 2 +- src/main_components/graph.jl | 22 +- src/main_components/mechanism_struct.jl | 91 +------- src/ui/urdf.jl | 245 ++++++++++++++++++++-- test/example_test.jl | 2 +- 6 files changed, 257 insertions(+), 107 deletions(-) diff --git a/benchmark/example_benchmark.jl b/benchmark/example_benchmark.jl index 00179177..9ef03277 100644 --- a/benchmark/example_benchmark.jl +++ b/benchmark/example_benchmark.jl @@ -46,7 +46,7 @@ for i=1:length(files) steps = Base.OneTo(100) storage = Storage{Float64}(steps,length(mech.bodies)) - if files[i] in controlled + if files[i] ∈ controlled control_function = eval(Meta.parse(files[i]*"_control!")) SUITE[files[i]] = @benchmarkable simulate!($mech, $steps, $storage, $control_function) samples=2 else diff --git a/src/main_components/equalityconstraint.jl b/src/main_components/equalityconstraint.jl index 3245817d..95122930 100644 --- a/src/main_components/equalityconstraint.jl +++ b/src/main_components/equalityconstraint.jl @@ -207,7 +207,7 @@ function Base.cat(eqc1::EqualityConstraint{T,N1,Nc1}, eqc2::EqualityConstraint{T constraints = [[eqc1.constraints[i] for i=1:Nc1];[eqc2.constraints[i] for i=1:Nc2]] childids = [[eqc1.childids[i] for i=1:Nc1];[eqc2.childids[i] for i=1:Nc2]] - eqc = EqualityConstraint([(constraints[i],parentid,childids[i]) for i=1:Nc1+Nc2]..., name="combined_"*eqc1.name*eqc2.name) + eqc = EqualityConstraint([(constraints[i],parentid,childids[i]) for i=1:Nc1+Nc2]..., name="combined_"*eqc1.name*"_and_"*eqc2.name) nothingflag && (eqc.parentid = nothing) return eqc diff --git a/src/main_components/graph.jl b/src/main_components/graph.jl index 38b48b3f..3b094b66 100644 --- a/src/main_components/graph.jl +++ b/src/main_components/graph.jl @@ -1,16 +1,16 @@ struct Graph{N} - directchildren::Vector{Vector{Int64}} - loopchildren::Vector{Vector{Int64}} + directchildren::Vector{Vector{Int64}} # direct successor nodes + loopchildren::Vector{Vector{Int64}} # successor nodes excluding direct successors ineqchildren::Vector{Vector{Int64}} - successors::Vector{Vector{Int64}} # contains direct and loop children - predecessors::Vector{Vector{Int64}} - connections::Vector{Vector{Int64}} + successors::Vector{Vector{Int64}} # direct and loop children + predecessors::Vector{Vector{Int64}} # direct parent and loop-opening predecessor(s?) (for numerics?) + connections::Vector{Vector{Int64}} # direct connections - dfslist::SVector{N,Int64} - rdfslist::SVector{N,Int64} + dfslist::SVector{N,Int64} # depth-first-seach list (dfslist[end] = root) + rdfslist::SVector{N,Int64} # reverse dfslist - dict::UnitDict{Base.OneTo{Int64},Int64} - rdict::UnitDict{Base.OneTo{Int64},Int64} + dict::UnitDict{Base.OneTo{Int64},Int64} # maps ids to the graph-interal numbering (not dfs order) + rdict::UnitDict{Base.OneTo{Int64},Int64} # reverse mapping activedict::UnitDict{Base.OneTo{Int64},Bool} function Graph(origin::Origin,bodies::Vector{<:Body}, @@ -78,6 +78,7 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, graph::Graph{N}) wh end +### Construction functions function adjacencyMatrix(eqconstraints::Vector{<:EqualityConstraint}, bodies::Vector{<:Body}) A = zeros(Bool, 0, 0) dict = Dict{Int64,Int64}() @@ -183,6 +184,9 @@ function parent(dfsgraph::Matrix, dict::Dict, childid::Integer) return -1 end + +### Graph functions + # this is done in order! function successors(dfslist, pattern, dict::Dict) N = length(dfslist) diff --git a/src/main_components/mechanism_struct.jl b/src/main_components/mechanism_struct.jl index 075cf25b..c9e124ee 100644 --- a/src/main_components/mechanism_struct.jl +++ b/src/main_components/mechanism_struct.jl @@ -38,7 +38,11 @@ mutable struct Mechanism{T,Nn,Nb,Ne,Ni} <: AbstractMechanism{T,Nn,Nb,Ne,Ni} Ne = length(eqcs) Ni = length(ineqcs) Nn = Nb + Ne - # steps = Int64(ceil(tend / Δt)) + + if Nb < Ne + @info "More constraints than bodies. Potentially bad behavior." + end + currentid = 1 @@ -141,88 +145,9 @@ mutable struct Mechanism{T,Nn,Nb,Ne,Ni} <: AbstractMechanism{T,Nn,Nb,Ne,Ni} end function Mechanism(filename::AbstractString; floating::Bool=false, type::Type{T} = Float64, Δt::Real = .01, g::Real = -9.81) where T - origin, links, joints, shapes = parse_urdf(filename, floating, T) - - mechanism = Mechanism(origin, links, joints, shapes = shapes, Δt = Δt, g = g) - - graph = mechanism.graph - xjointlist = Dict{Int64,SVector{3,T}}() # stores id, x in world frame - qjointlist = Dict{Int64,UnitQuaternion{T}}() # stores id, q in world frame - - for id in graph.rdfslist # from root to leaves - component = getcomponent(mechanism, id) - if component isa Body - body = component - xbodylocal = body.state.xc - qbodylocal = body.state.qc - shape = getshape(shapes, id) - - preds = predecessors(graph, id); @assert length(preds) == 1 - parentid = preds[1] - constraint = geteqconstraint(mechanism, parentid); @assert length(constraint.constraints) == 2 - grandpreds = predecessors(graph, parentid); @assert length(grandpreds) ∈ [0;1] - - if length(grandpreds) == 1 # predecessor is link - grandparentid = grandpreds[1] - - parentbody = getbody(mechanism, grandparentid) - grandgrandpreds = predecessors(graph, grandparentid); @assert length(grandgrandpreds) == 1 - grandgrandparentid = grandgrandpreds[1] - parentconstraint = geteqconstraint(mechanism, grandgrandparentid); @assert length(parentconstraint.constraints) == 2 - - xparentbody = parentbody.state.xc # in world frame - qparentbody = parentbody.state.qc # in world frame - - xparentjoint = xjointlist[parentconstraint.id] # in world frame - qparentjoint = qjointlist[parentconstraint.id] # in world frame - else # predecessor is origin - parentbody = origin - - xparentbody = SA{T}[0; 0; 0] - qparentbody = one(UnitQuaternion{T}) - - xparentjoint = SA{T}[0; 0; 0] - qparentjoint = one(UnitQuaternion{T}) - end - - # urdf joint's x and q in parent's (parentbody) frame - xjointlocal = vrotate(xparentjoint + vrotate(constraint.constraints[1].vertices[1], qparentjoint) - xparentbody, inv(qparentbody)) - qjointlocal = qparentbody \ qparentjoint * constraint.constraints[2].qoffset - - # store joint's x and q in world frame - xjoint = xparentbody + vrotate(xjointlocal, qparentbody) - qjoint = qparentbody * qjointlocal - xjointlist[constraint.id] = xjoint - qjointlist[constraint.id] = qjoint - - # difference to parent body (parentbody) - qoffset = qjointlocal * qbodylocal - - # actual joint properties - p1 = xjointlocal # in parent's (parentbody) frame - p2 = vrotate(-xbodylocal, inv(qbodylocal)) # in body frame (xbodylocal and qbodylocal are both relative to the same (joint) frame -> rotationg by inv(body.q) gives body frame) - constraint.constraints[1].vertices = (p1, p2) - - V3 = vrotate(constraint.constraints[2].V3', qjointlocal) # in parent's (parentbody) frame - V12 = (svd(skew(V3)).Vt)[1:2,:] - constraint.constraints[2].V3 = V3' - constraint.constraints[2].V12 = V12 - constraint.constraints[2].qoffset = qoffset # in parent's (parentbody) frame - - # actual body properties - setPosition!(body) # set everything to zero - setPosition!(parentbody, body, p1 = p1, p2 = p2, Δq = qoffset) - xbody = body.state.xc - qbody = body.state.qc - - - # shape relative - if shape !== nothing - shape.xoffset = vrotate(xjoint + vrotate(shape.xoffset, qjoint) - xbody, inv(qbody)) - shape.qoffset = qoffset \ qjointlocal * shape.qoffset - end - end - end + origin, links, joints, loopjoints, x2q2list, shapes = parse_urdf(filename, floating, T) + mechanism = Mechanism(origin, links, [joints;loopjoints], shapes = shapes, Δt = Δt, g = g) + set_parsed_values!(mechanism, loopjoints, shapes) return mechanism, shapes end diff --git a/src/ui/urdf.jl b/src/ui/urdf.jl index 57f494fb..01749e80 100644 --- a/src/ui/urdf.jl +++ b/src/ui/urdf.jl @@ -1,3 +1,5 @@ +### Before parsing + unsafeattribute(::Nothing, ::Core.AbstractString) = nothing unsafeattribute(x::LightXML.XMLElement, name::Core.AbstractString) = attribute(x, name) @@ -189,30 +191,62 @@ function parse_links(xlinks, materialdict, T) return ldict, shapes end -function parse_joint(xjoint, origin, plink, clink, T) +function parse_joint(xjoint, plink, clink, T) jointtype = attribute(xjoint, "type") x, q = parse_pose(find_element(xjoint, "origin"), T) axis = parse_vector(find_element(xjoint, "axis"), "xyz", T, default = "1 0 0") p1 = x - p2 = zeros(T, 3) name = attribute(xjoint, "name") # TODO limits for revolute joint? if jointtype == "revolute" || jointtype == "continuous" - joint = EqualityConstraint(Revolute(plink, clink, axis; p1=p1, p2=p2, qoffset = q), name=name) + joint = EqualityConstraint(Revolute(plink, clink, axis; p1=p1, qoffset = q), name=name) elseif jointtype == "prismatic" - joint = EqualityConstraint(Prismatic(plink, clink, axis; p1=p1, p2=p2, qoffset = q), name=name) + joint = EqualityConstraint(Prismatic(plink, clink, axis; p1=p1, qoffset = q), name=name) elseif jointtype == "planar" - joint = EqualityConstraint(Planar(plink, clink, axis; p1=p1, p2=p2, qoffset = q), name=name) + joint = EqualityConstraint(Planar(plink, clink, axis; p1=p1, qoffset = q), name=name) elseif jointtype == "fixed" - joint = EqualityConstraint(Fixed(plink, clink; p1=p1, p2=p2, qoffset = q), name=name) + joint = EqualityConstraint(Fixed(plink, clink; p1=p1, qoffset = q), name=name) elseif jointtype == "floating" joint = EqualityConstraint(Floating(plink, clink), name=name) + else + @error "Unknown joint type" end return joint end +# TODO currently assuming q2 = 0. There is no convention, but having both q doesn't make sense anyways because. +function parse_loop_joint(xjoint, link1, link2, T) + find_element(xjoint, "link1") + find_element(xjoint, "link2") + + jointtype = attribute(xjoint, "type") + axis = parse_vector(find_element(xjoint, "axis"), "xyz", T, default = "1 0 0") + x1, q1 = parse_pose(find_element(xjoint, "link1"), T) + x2, q2 = parse_pose(find_element(xjoint, "link2"), T) + p1 = x1 + x2q2 = (x2,q2) + name = attribute(xjoint, "name") + + # TODO limits for revolute joint? + if jointtype == "revolute" || jointtype == "continuous" + joint = EqualityConstraint(Revolute(link1, link2, axis; p1=p1, qoffset = q1), name=name) + elseif jointtype == "prismatic" + joint = EqualityConstraint(Prismatic(link1, link2, axis; p1=p1, qoffset = q1), name=name) + elseif jointtype == "planar" + joint = EqualityConstraint(Planar(link1, link2, axis; p1=p1, qoffset = q1), name=name) + elseif jointtype == "fixed" + joint = EqualityConstraint(Fixed(link1, link2; p1=p1, qoffset = q1), name=name) + elseif jointtype == "floating" + joint = EqualityConstraint(Floating(link1, link2), name=name) + else + @error "Unknown joint type" + end + + return joint, x2q2 +end + function parse_joints(xjoints, ldict, floating, T) origins = Origin{T}[] links = Body{T}[] @@ -220,16 +254,16 @@ function parse_joints(xjoints, ldict, floating, T) floatingname = "" for name in keys(ldict) - child = false + childflag = false for xjoint in xjoints xchild = find_element(xjoint, "child") childname = attribute(xchild, "link") if childname == name - child = true + childflag = true break end end - if child + if childflag push!(links, ldict[name]) else origin = Origin{T}() @@ -256,18 +290,103 @@ function parse_joints(xjoints, ldict, floating, T) xclink = find_element(xjoint, "child") clink = ldict[attribute(xclink, "link")] - joint = parse_joint(xjoint, origin, plink, clink, T) + joint = parse_joint(xjoint, plink, clink, T) push!(joints, joint) end if floating - originjoint = EqualityConstraint(Floating(origin, ldict[floatingname]),name="autoorigincon") + originjoint = EqualityConstraint(Floating(origin, ldict[floatingname]), name="auto_generated_floating_joint") push!(joints, originjoint) end return origin, links, joints end +# TODO This might be missing the detection of a direct loop, i.e. only two links connected by two joints +function parse_loop_joints(xloopjoints, origin, joints, ldict, T) + loopjoints = EqualityConstraint{T}[] + x2q2list = Tuple{AbstractVector,UnitQuaternion}[] + + + for xloopjoint in xloopjoints + xlink1 = find_element(xloopjoint, "link1") + xlink2 = find_element(xloopjoint, "link2") + link1 = ldict[attribute(xlink1, "link")] + link2 = ldict[attribute(xlink2, "link")] + + predlist = Tuple{Int64,Int64}[] + jointlist = [(joints[i].id,joints[i].parentid,joints[i].childids) for i=1:length(joints)] + linkid = link1.id + + while true # create list of predecessor joints and parent links for link1 + for (i,jointdata) in enumerate(jointlist) + if linkid ∈ jointdata[3] + push!(predlist,(jointdata[1],jointdata[2])) + linkid = jointdata[2] + deleteat!(jointlist,i) + break + end + end + if linkid == origin.id + break + end + end + + jointlist = [(joints[i].id,joints[i].parentid,joints[i].childids) for i=1:length(joints)] + linkid = link2.id + joint1id = 0 + joint2id = 0 + foundflag = false + + while true # check which predecessor of link2 is also a predecessor of link1 + for (i,jointdata) in enumerate(jointlist) + if linkid ∈ jointdata[3] + joint2id = jointdata[1] + linkid = jointdata[2] + deleteat!(jointlist,i) + break + end + end + for el in predlist + if linkid == el[2] + joint1id = el[1] + foundflag = true + break + end + end + foundflag && break + end + + # Find and remove joints to combine them + joint1 = 0 + joint2 = 0 + for (i,joint) in enumerate(joints) + if joint.id == joint1id + joint1 = joint + deleteat!(joints,i) + break + end + end + for (i,joint) in enumerate(joints) + if joint.id == joint2id + joint2 = joint + deleteat!(joints,i) + break + end + end + + display(joint1) + display(joint2) + joint = cat(joint1,joint2) + push!(joints,joint) + loopjoint, x2q2 = parse_loop_joint(xloopjoint, link1, link2, T) + push!(loopjoints, loopjoint) + push!(x2q2list, x2q2) + end + + return joints, loopjoints, x2q2list +end + function parse_urdf(filename, floating, ::Type{T}) where T xdoc = LightXML.parse_file(filename) xroot = LightXML.root(xdoc) @@ -275,12 +394,114 @@ function parse_urdf(filename, floating, ::Type{T}) where T xlinks = get_elements_by_tagname(xroot, "link") xjoints = get_elements_by_tagname(xroot, "joint") + xloopjoints = get_elements_by_tagname(xroot, "loop_joint") materialdict = parse_robotmaterials(xroot, T) ldict, shapes = parse_links(xlinks, materialdict, T) origin, links, joints = parse_joints(xjoints, ldict, floating, T) + joints, loopjoints, x2q2list = parse_loop_joints(xloopjoints, origin, joints, ldict, T) + free(xdoc) - return origin, links, joints, shapes + return origin, links, joints, loopjoints, x2q2list, shapes +end + + +### After parsing + +function set_parsed_values!(mechanism::Mechanism{T}, loopjoints, shapes) where T + graph = mechanism.graph + xjointlist = Dict{Int64,SVector{3,T}}() # stores id, x in world frame + qjointlist = Dict{Int64,UnitQuaternion{T}}() # stores id, q in world frame + + for id in graph.rdfslist # from root to leaves + component = getcomponent(mechanism, id) + !(component isa Body) && continue # only for bodies + + body = component + xbodylocal = body.state.xc + qbodylocal = body.state.qc + shape = getshape(shapes, id) + + parentid = get_parentid(mechanism, id, loopjoints) + constraint = geteqconstraint(mechanism, parentid) + + grandparentid = constraint.parentid + if grandparentid === nothing # predecessor is origin + parentbody = mechanism.origin + + xparentbody = SA{T}[0; 0; 0] + qparentbody = one(UnitQuaternion{T}) + + xparentjoint = SA{T}[0; 0; 0] + qparentjoint = one(UnitQuaternion{T}) + else + parentbody = getbody(mechanism, grandparentid) + + grandgrandparentid = get_parentid(mechanism, grandparentid, loopjoints) + parentconstraint = geteqconstraint(mechanism, grandgrandparentid) + + xparentbody = parentbody.state.xc # in world frame + qparentbody = parentbody.state.qc # in world frame + + xparentjoint = xjointlist[parentconstraint.id] # in world frame + qparentjoint = qjointlist[parentconstraint.id] # in world frame + end + + ind1 = findfirst(x->x==id,constraint.childids) + ind2 = ind1+1 + + # urdf joint's x and q in parent's (parentbody) frame + xjointlocal = vrotate(xparentjoint + vrotate(constraint.constraints[ind1].vertices[1], qparentjoint) - xparentbody, inv(qparentbody)) + qjointlocal = qparentbody \ qparentjoint * constraint.constraints[ind2].qoffset + + # store joint's x and q in world frame + xjoint = xparentbody + vrotate(xjointlocal, qparentbody) + qjoint = qparentbody * qjointlocal + xjointlist[constraint.id] = xjoint + qjointlist[constraint.id] = qjoint + + # difference to parent body (parentbody) + qoffset = qjointlocal * qbodylocal + + # actual joint properties + p1 = xjointlocal # in parent's (parentbody) frame + p2 = vrotate(-xbodylocal, inv(qbodylocal)) # in body frame (xbodylocal and qbodylocal are both relative to the same (joint) frame -> rotationg by inv(body.q) gives body frame) + constraint.constraints[ind1].vertices = (p1, p2) + + V3 = vrotate(constraint.constraints[ind2].V3', qjointlocal) # in parent's (parentbody) frame + V12 = (svd(skew(V3)).Vt)[1:2,:] + constraint.constraints[ind2].V3 = V3' + constraint.constraints[ind2].V12 = V12 + constraint.constraints[ind2].qoffset = qoffset # in parent's (parentbody) frame + + # actual body properties + setPosition!(body) # set everything to zero + setPosition!(parentbody, body, p1 = p1, p2 = p2, Δq = qoffset) + xbody = body.state.xc + qbody = body.state.qc + + # shape relative + if shape !== nothing + shape.xoffset = vrotate(xjoint + vrotate(shape.xoffset, qjoint) - xbody, inv(qbody)) + shape.qoffset = qoffset \ qjointlocal * shape.qoffset + end + end + for constraint in loopjoints + + end +end + +function get_parentid(mechanism, id, loopjoints) + graph = mechanism.graph + conns = connections(graph, id) + for connsid in conns + constraint = geteqconstraint(mechanism, connsid) + if constraint ∉ loopjoints && id ∈ constraint.childids + return connsid + end + end + + return nothing end \ No newline at end of file diff --git a/test/example_test.jl b/test/example_test.jl index 4addd8f7..acbb1651 100644 --- a/test/example_test.jl +++ b/test/example_test.jl @@ -43,7 +43,7 @@ controlled = [ for file in files include("examples/"*file*".jl") - if file in controlled + if file ∈ controlled storage = simulate!(mech, 10., eval(Meta.parse(file*"_control!")), record = true) else storage = simulate!(mech, 10., record = true) From beed8d16bbd30c004bde14cbeac1d75fc938f2f2 Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Tue, 1 Dec 2020 16:33:57 +0100 Subject: [PATCH 3/5] Looped urdf for simple mechanism --- src/main_components/mechanism_struct.jl | 2 +- src/ui/urdf.jl | 84 ++++++++++++++++++++----- 2 files changed, 70 insertions(+), 16 deletions(-) diff --git a/src/main_components/mechanism_struct.jl b/src/main_components/mechanism_struct.jl index c9e124ee..75b3b500 100644 --- a/src/main_components/mechanism_struct.jl +++ b/src/main_components/mechanism_struct.jl @@ -145,7 +145,7 @@ mutable struct Mechanism{T,Nn,Nb,Ne,Ni} <: AbstractMechanism{T,Nn,Nb,Ne,Ni} end function Mechanism(filename::AbstractString; floating::Bool=false, type::Type{T} = Float64, Δt::Real = .01, g::Real = -9.81) where T - origin, links, joints, loopjoints, x2q2list, shapes = parse_urdf(filename, floating, T) + origin, links, joints, loopjoints, shapes = parse_urdf(filename, floating, T) mechanism = Mechanism(origin, links, [joints;loopjoints], shapes = shapes, Δt = Δt, g = g) set_parsed_values!(mechanism, loopjoints, shapes) diff --git a/src/ui/urdf.jl b/src/ui/urdf.jl index 01749e80..be65ac0a 100644 --- a/src/ui/urdf.jl +++ b/src/ui/urdf.jl @@ -216,7 +216,6 @@ function parse_joint(xjoint, plink, clink, T) return joint end -# TODO currently assuming q2 = 0. There is no convention, but having both q doesn't make sense anyways because. function parse_loop_joint(xjoint, link1, link2, T) find_element(xjoint, "link1") find_element(xjoint, "link2") @@ -224,27 +223,27 @@ function parse_loop_joint(xjoint, link1, link2, T) jointtype = attribute(xjoint, "type") axis = parse_vector(find_element(xjoint, "axis"), "xyz", T, default = "1 0 0") x1, q1 = parse_pose(find_element(xjoint, "link1"), T) - x2, q2 = parse_pose(find_element(xjoint, "link2"), T) + x2, q2 = parse_pose(find_element(xjoint, "link2"), T) # The orientation q2 of the second body is ignored because it is determined by the mechanism's structure p1 = x1 - x2q2 = (x2,q2) + p2 = x2 name = attribute(xjoint, "name") # TODO limits for revolute joint? if jointtype == "revolute" || jointtype == "continuous" - joint = EqualityConstraint(Revolute(link1, link2, axis; p1=p1, qoffset = q1), name=name) + joint = EqualityConstraint(Revolute(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name) elseif jointtype == "prismatic" - joint = EqualityConstraint(Prismatic(link1, link2, axis; p1=p1, qoffset = q1), name=name) + joint = EqualityConstraint(Prismatic(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name) elseif jointtype == "planar" - joint = EqualityConstraint(Planar(link1, link2, axis; p1=p1, qoffset = q1), name=name) + joint = EqualityConstraint(Planar(link1, link2, axis; p1=p1, p2=p2, qoffset = q1), name=name) elseif jointtype == "fixed" - joint = EqualityConstraint(Fixed(link1, link2; p1=p1, qoffset = q1), name=name) + joint = EqualityConstraint(Fixed(link1, link2; p1=p1, p2=p2, qoffset = q1), name=name) elseif jointtype == "floating" joint = EqualityConstraint(Floating(link1, link2), name=name) else @error "Unknown joint type" end - return joint, x2q2 + return joint end function parse_joints(xjoints, ldict, floating, T) @@ -305,7 +304,6 @@ end # TODO This might be missing the detection of a direct loop, i.e. only two links connected by two joints function parse_loop_joints(xloopjoints, origin, joints, ldict, T) loopjoints = EqualityConstraint{T}[] - x2q2list = Tuple{AbstractVector,UnitQuaternion}[] for xloopjoint in xloopjoints @@ -379,12 +377,11 @@ function parse_loop_joints(xloopjoints, origin, joints, ldict, T) display(joint2) joint = cat(joint1,joint2) push!(joints,joint) - loopjoint, x2q2 = parse_loop_joint(xloopjoint, link1, link2, T) + loopjoint = parse_loop_joint(xloopjoint, link1, link2, T) push!(loopjoints, loopjoint) - push!(x2q2list, x2q2) end - return joints, loopjoints, x2q2list + return joints, loopjoints end function parse_urdf(filename, floating, ::Type{T}) where T @@ -400,11 +397,11 @@ function parse_urdf(filename, floating, ::Type{T}) where T ldict, shapes = parse_links(xlinks, materialdict, T) origin, links, joints = parse_joints(xjoints, ldict, floating, T) - joints, loopjoints, x2q2list = parse_loop_joints(xloopjoints, origin, joints, ldict, T) + joints, loopjoints = parse_loop_joints(xloopjoints, origin, joints, ldict, T) free(xdoc) - return origin, links, joints, loopjoints, x2q2list, shapes + return origin, links, joints, loopjoints, shapes end @@ -488,8 +485,65 @@ function set_parsed_values!(mechanism::Mechanism{T}, loopjoints, shapes) where T shape.qoffset = qoffset \ qjointlocal * shape.qoffset end end - for constraint in loopjoints + for (i,constraint) in enumerate(loopjoints) + @assert length(constraint.childids) == 2 # Loop joint connects only two bodies + + parentid1 = constraint.parentid + parentid2 = constraint.childids[1] + if parentid1 === nothing # predecessor is origin + parentbody1 = mechanism.origin + + xparentbody1 = SA{T}[0; 0; 0] + qparentbody1 = one(UnitQuaternion{T}) + + xparentjoint1 = SA{T}[0; 0; 0] + qparentjoint1 = one(UnitQuaternion{T}) + else + parentbody1 = getbody(mechanism, parentid1) + + grandparentid1 = get_parentid(mechanism, parentid1, loopjoints) + parentconstraint1 = geteqconstraint(mechanism, grandparentid1) + + xparentbody1 = parentbody1.state.xc # in world frame + qparentbody1 = parentbody1.state.qc # in world frame + + xparentjoint1 = xjointlist[parentconstraint1.id] # in world frame + qparentjoint1 = qjointlist[parentconstraint1.id] # in world frame + end + parentbody2 = getbody(mechanism, parentid2) + + grandparentid2 = get_parentid(mechanism, parentid2, loopjoints) + parentconstraint2 = geteqconstraint(mechanism, grandparentid2) + + xparentbody2 = parentbody2.state.xc # in world frame + qparentbody2 = parentbody2.state.qc # in world frame + + xparentjoint2 = xjointlist[parentconstraint2.id] # in world frame + qparentjoint2 = qjointlist[parentconstraint2.id] # in world frame + + ind1 = 1 + ind2 = ind1+1 + + + # urdf joint's x and q in parent's (parentbody) frame + xjointlocal1 = vrotate(xparentjoint1 + vrotate(constraint.constraints[ind1].vertices[1], qparentjoint1) - xparentbody1, inv(qparentbody1)) + xjointlocal2 = vrotate(xparentjoint2 + vrotate(constraint.constraints[ind1].vertices[2], qparentjoint2) - xparentbody2, inv(qparentbody2)) + qjointlocal1 = qparentbody1 \ qparentjoint1 * constraint.constraints[ind2].qoffset + + # difference to parent body (parentbody) + qoffset1 = qjointlocal1 * qparentbody2 # qparentbody2 = body in for loop above + + # actual joint properties + p1 = xjointlocal1 # in parent's (parentbody1) frame + p2 = xjointlocal2 # in parent's (parentbody2) frame + constraint.constraints[ind1].vertices = (p1, p2) + + V3 = vrotate(constraint.constraints[ind2].V3', qjointlocal1) # in parent's (parentbody1) frame + V12 = (svd(skew(V3)).Vt)[1:2,:] + constraint.constraints[ind2].V3 = V3' + constraint.constraints[ind2].V12 = V12 + constraint.constraints[ind2].qoffset = qoffset1 # in parent's (parentbody1) frame end end From 1ce2167f475b1ab18684e404d0d95af4ad12f7de Mon Sep 17 00:00:00 2001 From: Jan Bruedigam Date: Tue, 1 Dec 2020 17:52:12 +0100 Subject: [PATCH 4/5] Save current state --- examples/examples_files/fourbar.urdf | 115 + examples/examples_files/strandbeest.urdf | 2545 ++++++++++++++++++++++ examples/fourbar_urdf.jl | 14 + examples/strandbeest.jl | 16 + src/ui/urdf.jl | 9 +- 5 files changed, 2695 insertions(+), 4 deletions(-) create mode 100644 examples/examples_files/fourbar.urdf create mode 100644 examples/examples_files/strandbeest.urdf create mode 100644 examples/fourbar_urdf.jl create mode 100644 examples/strandbeest.jl diff --git a/examples/examples_files/fourbar.urdf b/examples/examples_files/fourbar.urdf new file mode 100644 index 00000000..0753216c --- /dev/null +++ b/examples/examples_files/fourbar.urdf @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/examples_files/strandbeest.urdf b/examples/examples_files/strandbeest.urdf new file mode 100644 index 00000000..06ab87d2 --- /dev/null +++ b/examples/examples_files/strandbeest.urdf @@ -0,0 +1,2545 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/fourbar_urdf.jl b/examples/fourbar_urdf.jl new file mode 100644 index 00000000..8bdba4c6 --- /dev/null +++ b/examples/fourbar_urdf.jl @@ -0,0 +1,14 @@ +using ConstrainedDynamics +using ConstrainedDynamicsVis + + +path = "examples/examples_files/fourbar.urdf" +mech, shapes = Mechanism(path) + +initializeConstraints!(mech, newtonIter=1000) + +steps = Base.OneTo(1000) +storage = Storage{Float64}(steps,4) + +storage = simulate!(mech, storage, record = true) +visualize(mech, storage, shapes) diff --git a/examples/strandbeest.jl b/examples/strandbeest.jl new file mode 100644 index 00000000..bf8daeb6 --- /dev/null +++ b/examples/strandbeest.jl @@ -0,0 +1,16 @@ +using ConstrainedDynamics +using ConstrainedDynamicsVis + + +path = "examples/examples_files/strandbeest.urdf" +mech, shapes = Mechanism(path, floating=true, g = 0) + +v = rand.(length.(minimalCoordinates(mech).values)) +# d = ConstrainedDynamics.UnitDict(92:92+90,v) +d = ConstrainedDynamics.UnitDict(93:93+91,v) +setPosition!(mech,d) + +storage = simulate!(mech, 10., record = true) +visualize(mech, storage, shapes) + + diff --git a/src/ui/urdf.jl b/src/ui/urdf.jl index be65ac0a..e4f0f126 100644 --- a/src/ui/urdf.jl +++ b/src/ui/urdf.jl @@ -301,7 +301,8 @@ function parse_joints(xjoints, ldict, floating, T) return origin, links, joints end -# TODO This might be missing the detection of a direct loop, i.e. only two links connected by two joints +# TODO This might be missing the detection of a direct loop, i.e. only two links connected by two joints +# TODO Also only works for a single loop closure in a cycle (so no ladders) function parse_loop_joints(xloopjoints, origin, joints, ldict, T) loopjoints = EqualityConstraint{T}[] @@ -336,7 +337,7 @@ function parse_loop_joints(xloopjoints, origin, joints, ldict, T) joint2id = 0 foundflag = false - while true # check which predecessor of link2 is also a predecessor of link1 + while true # check which predecessor link of link2 is also a predecessor link of link1 for (i,jointdata) in enumerate(jointlist) if linkid ∈ jointdata[3] joint2id = jointdata[1] @@ -365,6 +366,8 @@ function parse_loop_joints(xloopjoints, origin, joints, ldict, T) break end end + # if joint1id == joint2id # already combined joint + # joint2 = joint for (i,joint) in enumerate(joints) if joint.id == joint2id joint2 = joint @@ -373,8 +376,6 @@ function parse_loop_joints(xloopjoints, origin, joints, ldict, T) end end - display(joint1) - display(joint2) joint = cat(joint1,joint2) push!(joints,joint) loopjoint = parse_loop_joint(xloopjoint, link1, link2, T) From b60f84b029e8a829ae2a521d7491af14029ef090 Mon Sep 17 00:00:00 2001 From: janbruedigam Date: Wed, 6 Jan 2021 18:30:24 +0100 Subject: [PATCH 5/5] Add comment to example --- examples/strandbeest.jl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/strandbeest.jl b/examples/strandbeest.jl index bf8daeb6..80a1d72b 100644 --- a/examples/strandbeest.jl +++ b/examples/strandbeest.jl @@ -1,3 +1,5 @@ +### Does not work yet! + using ConstrainedDynamics using ConstrainedDynamicsVis