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

@@ -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
 

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

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)

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))

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: ×
 

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