From c66609bbe365f6d6aa4677094367f59b1d41844f Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Wed, 2 Jan 2019 11:55:05 -0500 Subject: [PATCH] Stop using canonicalize_graph! in MechanismState ctor (and write_urdf). Using canocalize_graph! in the MechanismState constructor was necessary because removing a tree joint can make it so that the indices of the tree joints are no longer ordered and contiguous. However, it made it so that constructing MechanismStates was not thread-safe. In addition, it was perhaps an unexpected (although mostly harmless) behavior to have the Mechanism be modified in any way upon MechanismState construction or in write_urdf. Instead, canonicalize the graph in remove_joint! and remove_fixed_tree_joints!. --- src/graphs/spanning_tree.jl | 8 +++++--- src/graphs/tree_path.jl | 2 ++ src/mechanism_modification.jl | 21 +++++++++++++++------ src/mechanism_state.jl | 8 +++++--- src/urdf/URDF.jl | 2 +- src/urdf/write.jl | 16 ++++++---------- 6 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/graphs/spanning_tree.jl b/src/graphs/spanning_tree.jl index 28c838d3..2572f56e 100644 --- a/src/graphs/spanning_tree.jl +++ b/src/graphs/spanning_tree.jl @@ -19,7 +19,7 @@ root(tree::SpanningTree) = tree.root edge_to_parent(vertex::V, tree::SpanningTree{V, E}) where {V, E} = tree.inedges[vertex_index(vertex)] edges_to_children(vertex::V, tree::SpanningTree{V, E}) where {V, E} = out_edges(vertex, tree) tree_index(edge, tree::SpanningTree{V, E}) where {V, E} = tree.edge_tree_indices[edge_index(edge)] -tree_index(vertex::V, tree::SpanningTree{V, E}) where {V, E} = vertex == root(tree) ? 1 : tree_index(edge_to_parent(vertex, tree), tree) + 1 +tree_index(vertex::V, tree::SpanningTree{V, E}) where {V, E} = vertex === root(tree) ? 1 : tree_index(edge_to_parent(vertex, tree), tree) + 1 function SpanningTree(g::DirectedGraph{V, E}, root::V, edges::AbstractVector{E}) where {V, E} n = num_vertices(g) @@ -42,7 +42,7 @@ function SpanningTree(g::DirectedGraph{V, E}, root::V, edges::AbstractVector{E}) SpanningTree(g, root, edges, inedges, outedges, edge_tree_indices) end -function SpanningTree(g::DirectedGraph{V, E}, root::V, flipped_edge_map::AbstractDict = Dict{E, E}(); +function SpanningTree(g::DirectedGraph{V, E}, root::V, flipped_edge_map::Union{AbstractDict, Nothing} = nothing; next_edge = first #= breadth first =#) where {V, E} tree_edges = E[] tree_vertices = [root] @@ -64,7 +64,9 @@ function SpanningTree(g::DirectedGraph{V, E}, root::V, flipped_edge_map::Abstrac rewire!(g, e, target(e, g), source(e, g)) newedge = flip_direction(e) replace_edge!(g, e, newedge) - flipped_edge_map[e] = newedge + if flipped_edge_map !== nothing + flipped_edge_map[e] = newedge + end e = newedge end diff --git a/src/graphs/tree_path.jl b/src/graphs/tree_path.jl index deeab04a..25344e86 100644 --- a/src/graphs/tree_path.jl +++ b/src/graphs/tree_path.jl @@ -47,10 +47,12 @@ function TreePath(src::V, target::V, tree::SpanningTree{V, E}) where {V, E} if tree_index(source_current, tree) > tree_index(target_current, tree) edge = edge_to_parent(source_current, tree) push!(source_to_lca, edge) + length(source_to_lca) > 100 && error() source_current = source(edge, tree) else edge = edge_to_parent(target_current, tree) push!(lca_to_target, edge) + length(lca_to_target) > 100 && error() target_current = source(edge, tree) end end diff --git a/src/mechanism_modification.jl b/src/mechanism_modification.jl index 9ee75010..1ac1c5cd 100644 --- a/src/mechanism_modification.jl +++ b/src/mechanism_modification.jl @@ -147,11 +147,12 @@ Also optionally, `next_edge` can be used to select which joints should become pa new spanning tree. """ function rebuild_spanning_tree!(mechanism::Mechanism{M}, - flipped_joint_map::AbstractDict = Dict{<:Joint{M}, <:Joint{M}}(); + flipped_joint_map::Union{AbstractDict, Nothing} = nothing; next_edge = first #= breadth first =#) where {M} mechanism.tree = SpanningTree(mechanism.graph, root_body(mechanism), flipped_joint_map; next_edge = next_edge) register_modification!(mechanism) canonicalize_frame_definitions!(mechanism) + mechanism end """ @@ -168,12 +169,17 @@ Also optionally, `spanning_tree_next_edge` can be used to select which joints sh new spanning tree, if rebuilding the spanning tree is required. """ function remove_joint!(mechanism::Mechanism{M}, joint::Joint{M}; - flipped_joint_map::AbstractDict = Dict{Joint{M}, Joint{M}}(), + flipped_joint_map::Union{AbstractDict, Nothing} = nothing, spanning_tree_next_edge = first #= breadth first =#) where {M} istreejoint = joint ∈ tree_joints(mechanism) remove_edge!(mechanism.graph, joint) register_modification!(mechanism) - istreejoint && rebuild_spanning_tree!(mechanism, flipped_joint_map; next_edge = spanning_tree_next_edge) + if istreejoint + rebuild_spanning_tree!(mechanism, flipped_joint_map, + next_edge=spanning_tree_next_edge) # also recanonicalizes frame definitions + canonicalize_graph!(mechanism) + end + nothing end function replace_joint!(mechanism::Mechanism, oldjoint::Joint, newjoint::Joint) @@ -244,8 +250,9 @@ function remove_fixed_tree_joints!(mechanism::Mechanism) # Recompute spanning tree (preserves order for non-fixed joints) mechanism.tree = SpanningTree(graph, root_body(mechanism), newtreejoints) - # Recanonicalize frames + # Recanonicalize graph and frames canonicalize_frame_definitions!(mechanism) + canonicalize_graph!(mechanism) register_modification!(mechanism) @@ -298,10 +305,12 @@ end function canonicalize_graph!(mechanism::Mechanism) root = root_body(mechanism) treejoints = copy(tree_joints(mechanism)) - vertices = append!([root], successor(joint, mechanism) for joint in treejoints) edges = vcat(treejoints, non_tree_joints(mechanism)) + vertices = append!([root], successor(joint, mechanism) for joint in treejoints) reindex!(mechanism.graph, vertices, edges) - mechanism.tree = SpanningTree(mechanism.graph, root, treejoints) + mechanism.tree = SpanningTree(mechanism.graph, root, treejoints) # otherwise tree.edge_tree_indices can go out of sync + register_modification!(mechanism) + mechanism end add_environment_primitive!(mechanism::Mechanism, halfspace::HalfSpace3D) = push!(mechanism.environment, halfspace) diff --git a/src/mechanism_state.jl b/src/mechanism_state.jl index 19019fbe..d81f29a8 100644 --- a/src/mechanism_state.jl +++ b/src/mechanism_state.jl @@ -82,7 +82,6 @@ struct MechanismState{X, M, C, JointCollection} @assert length(s) == num_additional_states(m) # mechanism layout - canonicalize_graph!(m) nonrootbodies = collect(non_root_bodies(m)) treejoints = JointCollection(tree_joints(m)) nontreejoints = JointCollection(non_tree_joints(m)) @@ -93,8 +92,11 @@ struct MechanismState{X, M, C, JointCollection} nontreejointids = lasttreejointid + 1 : lastjointid predecessor_and_successor_ids = JointDict{Pair{BodyID, BodyID}}( JointID(j) => (BodyID(predecessor(j, m)) => BodyID(successor(j, m))) for j in joints(m)) - ancestor_joint_mask = joint -> JointDict{Bool}( - JointID(j) => j ∈ path(m, successor(joint, m), root_body(m)) for j in tree_joints(m)) + ancestor_joint_mask = function (joint) + JointDict{Bool}( + JointID(j) => j ∈ path(m, successor(joint, m), root_body(m)) for j in tree_joints(m) + ) + end ancestor_joint_masks = JointDict{JointDict{Bool}}(JointID(j) => ancestor_joint_mask(j) for j in tree_joints(m)) constraint_jacobian_structure = JointDict{TreePath{RigidBody{M}, Joint{M}}}( JointID(j) => path(m, predecessor(j, m), successor(j, m)) for j in joints(m)) diff --git a/src/urdf/URDF.jl b/src/urdf/URDF.jl index 7ba1e01b..b4776013 100644 --- a/src/urdf/URDF.jl +++ b/src/urdf/URDF.jl @@ -8,7 +8,7 @@ using DocStringExtensions using RigidBodyDynamics.Graphs using RigidBodyDynamics: Bounds, upper, lower -using RigidBodyDynamics: has_loops, canonicalize_graph!, joint_to_predecessor +using RigidBodyDynamics: has_loops, joint_to_predecessor using RigidBodyDynamics: DEFAULT_GRAVITATIONAL_ACCELERATION using LinearAlgebra: × diff --git a/src/urdf/write.jl b/src/urdf/write.jl index 9fbd8df4..62592d5d 100644 --- a/src/urdf/write.jl +++ b/src/urdf/write.jl @@ -120,23 +120,19 @@ end function to_urdf(mechanism::Mechanism; robot_name::Union{Nothing, AbstractString}=nothing, include_root::Bool=true) @assert !has_loops(mechanism) - - canonicalized = deepcopy(mechanism) - canonicalize_graph!(canonicalized) - xdoc = XMLDocument() xroot = create_root(xdoc, "robot") if robot_name !== nothing set_attribute(xroot, "name", robot_name) end - bodies_to_include = include_root ? bodies(canonicalized) : non_root_bodies(canonicalized) - for body in bodies(canonicalized) - !include_root && isroot(body, canonicalized) && continue + bodies_to_include = include_root ? bodies(mechanism) : non_root_bodies(mechanism) + for body in bodies(mechanism) + !include_root && isroot(body, mechanism) && continue add_child(xroot, to_urdf(body)) end - for joint in tree_joints(canonicalized) - !include_root && isroot(predecessor(joint, canonicalized), canonicalized) && continue - add_child(xroot, to_urdf(joint, canonicalized)) + for joint in tree_joints(mechanism) + !include_root && isroot(predecessor(joint, mechanism), mechanism) && continue + add_child(xroot, to_urdf(joint, mechanism)) end xdoc end