Merge remote-tracking branch 'JuliaRobotics/master' into hf/wrenches-…

…cache

Project.toml

@@ -1,6 +1,6 @@
 name = "RigidBodyDynamics"
 uuid = "366cf18f-59d5-5db9-a4de-86a9f6786172"
-version = "2.3.1"
+version = "2.3.2"
 
 [deps]
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"

src/RigidBodyDynamics.jl

@@ -88,7 +88,8 @@ export
     replace_joint!,
     maximal_coordinates,
     submechanism,
-    remove_fixed_tree_joints!
+    remove_fixed_tree_joints!,
+    remove_subtree!
 
 # contact-related functionality
 export # note: contact-related functionality may be changed significantly in the future

src/graphs/Graphs.jl

@@ -39,6 +39,7 @@ export
     tree_index,
     ancestors,
     lowest_common_ancestor,
+    subtree_vertices,
     direction,
     directions
 

src/graphs/spanning_tree.jl

@@ -82,7 +82,9 @@ function SpanningTree(g::DirectedGraph{V, E}, root::V, flipped_edge_map::Union{A
     SpanningTree(g, root, tree_edges)
 end
 
-# adds an edge and vertex to both the tree and the underlying graph
+"""
+Add an edge and vertex to both the tree and the underlying graph.
+"""
 function add_edge!(tree::SpanningTree{V, E}, source::V, target::V, edge::E) where {V, E}
     @assert target ∉ vertices(tree)
     add_edge!(tree.graph, source, target, edge)
@@ -96,6 +98,9 @@ function add_edge!(tree::SpanningTree{V, E}, source::V, target::V, edge::E) wher
     tree
 end
 
+"""
+Replace an edge in both the tree and the underlying graph.
+"""
 function replace_edge!(tree::SpanningTree{V, E}, old_edge::E, new_edge::E) where {V, E}
     @assert old_edge ∈ edges(tree)
     src = source(old_edge, tree)
@@ -145,3 +150,21 @@ function lowest_common_ancestor(v1::V, v2::V, tree::SpanningTree{V, E}) where {V
     end
     v1
 end
+
+"""
+Return a list of vertices in the subtree rooted at `subtree_root`, including `subtree_root` itself.
+The list is guaranteed to be topologically sorted.
+"""
+function subtree_vertices(subtree_root::V, tree::SpanningTree{V, E}) where {V, E}
+    @assert subtree_root ∈ vertices(tree)
+    frontier = [subtree_root]
+    subtree_vertices = V[]
+    while !isempty(frontier)
+        parent = pop!(frontier)
+        push!(subtree_vertices, parent)
+        for child in out_neighbors(parent, tree)
+            push!(frontier, child)
+        end
+    end
+    return subtree_vertices
+end

src/mechanism_modification.jl

@@ -152,6 +152,41 @@ function submechanism(mechanism::Mechanism{T}, submechanismroot::RigidBody{T};
     ret
 end
 
+"""
+Remove all bodies in the subtree rooted at `subtree_root`, including `subtree_root` itself,
+as well as all joints connected to these bodies.
+
+The ordering of the joints that remain in the mechanism is retained.
+"""
+function remove_subtree!(mechanism::Mechanism{T}, subtree_root::RigidBody{T}) where {T}
+    @assert subtree_root != root_body(mechanism)
+    tree = mechanism.tree
+    graph = mechanism.graph
+    bodies_to_remove = subtree_vertices(subtree_root, tree)
+    new_tree_joints = copy(edges(tree))
+    for body in bodies_to_remove
+        # Remove the tree joint from our new ordered list of joints.
+        tree_joint = edge_to_parent(body, tree)
+        deleteat!(new_tree_joints, findfirst(isequal(tree_joint), new_tree_joints))
+    end
+    for body in bodies_to_remove
+        # Remove all edges to and from the vertex in the graph.
+        for joint in copy(in_edges(body, graph))
+            remove_edge!(graph, joint)
+        end
+        for joint in copy(out_edges(body, graph))
+            remove_edge!(graph, joint)
+        end
+
+        # Remove the vertex itself.
+        remove_vertex!(graph, body)
+    end
+    # Construct a new spanning tree with the new list of tree joints.
+    mechanism.tree = SpanningTree(graph, root_body(mechanism), new_tree_joints)
+    canonicalize_graph!(mechanism)
+    mechanism
+end
+
 """
 $(SIGNATURES)
 

src/spatial/util.jl

@@ -91,11 +91,11 @@ function rotation_vector_rate(rotation_vector::AbstractVector{T}, angular_veloci
     ω = angular_velocity_in_body
     @boundscheck length(ϕ) == 3 || error("ϕ has wrong length")
     @boundscheck length(ω) == 3 || error("ω has wrong length")
+    ϕ̇ = ω + (ϕ × ω) / 2
     θ = norm(ϕ)
-    ϕ̇ = ω
-    if θ > eps(θ)
+    if θ > eps(typeof(θ))
         s, c = sincos(θ)
-        ϕ̇ += (ϕ × ω) / 2 + 1 / θ^2 * (1 - (θ * s) / (2 * (1 - c))) * ϕ × (ϕ × ω)
+        ϕ̇ += 1 / θ^2 * (1 - (θ * s) / (2 * (1 - c))) * ϕ × (ϕ × ω)
     end
     ϕ̇
 end

test/test_graph.jl

@@ -326,6 +326,25 @@ Graphs.flip_direction(edge::Edge{Int32}) = Edge(-edge.data)
         end
     end
 
+    @testset "subtree_vertices" begin
+        graph = DirectedGraph{Vertex{Int64}, Edge{Int32}}()
+        root = Vertex(0)
+        add_vertex!(graph, root)
+        tree = SpanningTree(graph, root)
+        for i = 1 : 30
+            parent = vertices(tree)[rand(1 : num_vertices(graph))]
+            child = Vertex(i)
+            edge = Edge(Int32(i + 3))
+            add_edge!(tree, parent, child, edge)
+        end
+        for subtree_root in vertices(tree)
+            subtree = subtree_vertices(subtree_root, tree)
+            for vertex in vertices(tree)
+                @test (vertex ∈ subtree) == (subtree_root ∈ ancestors(vertex, tree))
+            end
+        end
+    end
+
     @testset "reindex!" begin
         Random.seed!(15)
         graph = DirectedGraph{Vertex{Int64}, Edge{Float64}}()

test/test_mechanism_modification.jl

@@ -357,4 +357,55 @@
             showerror(devnull, e)
         end
     end
+
+    @testset "remove_subtree! - tree mechanism" begin
+        mechanism = parse_urdf(joinpath(@__DIR__, "urdf", "atlas.urdf"), floating=true)
+        @test_throws AssertionError remove_subtree!(mechanism, root_body(mechanism))
+
+        original_joints = copy(joints(mechanism))
+
+        # Behead.
+        num_bodies = length(bodies(mechanism))
+        num_joints = length(joints(mechanism))
+        head = findbody(mechanism, "head")
+        neck_joint = joint_to_parent(head, mechanism)
+        remove_subtree!(mechanism, head)
+        @test length(bodies(mechanism)) == num_bodies - 1
+        @test length(joints(mechanism)) == num_joints - 1
+        @test head ∉ bodies(mechanism)
+        @test neck_joint ∉ joints(mechanism)
+
+        # Lop off an arm.
+        num_bodies = length(bodies(mechanism))
+        num_joints = length(joints(mechanism))
+        r_clav = findbody(mechanism, "r_clav")
+        r_hand = findbody(mechanism, "r_hand")
+        r_arm = path(mechanism, r_clav, r_hand)
+        arm_joints = collect(r_arm)
+        arm_bodies = [r_clav; map(joint -> successor(joint, mechanism), arm_joints)]
+        remove_subtree!(mechanism, r_clav)
+        @test length(joints(mechanism)) == num_joints - length(arm_joints) - 1
+        @test length(bodies(mechanism)) == num_bodies - length(arm_bodies)
+        @test isempty(intersect(arm_joints, joints(mechanism)))
+        @test isempty(intersect(arm_bodies, bodies(mechanism)))
+        @test issorted(joints(mechanism), by=joint ->findfirst(isequal(joint), original_joints))
+    end
+
+    @testset "remove_subtree! - maximal coordinates" begin
+        original = parse_urdf(joinpath(@__DIR__, "urdf", "atlas.urdf"), floating=true)
+        mechanism = maximal_coordinates(original)
+        num_bodies = length(bodies(mechanism))
+        num_joints = length(joints(mechanism))
+        @test_throws AssertionError remove_subtree!(mechanism, findbody(original, "head")) # body not in tree
+        head = findbody(mechanism, "head")
+        head_joints = copy(in_joints(head, mechanism))
+        @test length(head_joints) == 2 # floating joint + neck loop joint
+        remove_subtree!(mechanism, head)
+        @test length(joints(mechanism)) == num_joints - length(head_joints)
+        @test length(bodies(mechanism)) == num_bodies - 1
+        for joint in head_joints
+            @test joint ∉ joints(mechanism)
+        end
+        @test head ∉ bodies(mechanism)
+    end
 end # mechanism modification

test/test_simulate.jl

@@ -225,4 +225,28 @@
         energy20 = gravitational_potential_energy(state) + kinetic_energy(state)
         @test energy20 ≈ energy15 atol=1e-5 # stabilization doesn't significantly affect energy after converging
     end
+
+    @testset "Ball joint pendulum" begin
+        # Issue #617
+        translation = [0.3, 0, 0]
+
+        world = RigidBody{Float64}("world")
+        pendulum = Mechanism(world; gravity=[0., 0., -9.81])
+
+        center_of_mass = [0, 0, 0.2]
+        q0 = [cos(pi/8), sin(pi/8), 0.0, 0.0]
+
+        joint1 = Joint("joint1", QuaternionSpherical{Float64}())
+        inertia1 = SpatialInertia(frame_after(joint1), com=center_of_mass, moment_about_com=diagm([1.,1.,1.]), mass=1.)
+        link1 = RigidBody("link1", inertia1)
+        before_joint1_to_world = Transform3D(frame_before(joint1), default_frame(world), one(RotMatrix{3}), SVector{3}(translation))
+        attach!(pendulum, world, link1, joint1, joint_pose=before_joint1_to_world)
+
+        state = MechanismState(pendulum)
+
+        set_configuration!(state, joint1, q0)
+        ts, qs, vs = simulate(state, 1., Δt=0.001)
+        @test all(all(!isnan, q) for q in qs)
+        @test all(all(!isnan, v) for v in vs)
+    end
 end