From d346d8c75bcfced0ec87191f93a512be2290068d Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Fri, 19 Mar 2021 17:52:35 -0400 Subject: [PATCH 1/4] Fix numerical epsilon check in rotation_vector_rate. --- src/spatial/util.jl | 2 +- test/test_simulate.jl | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/spatial/util.jl b/src/spatial/util.jl index 7f3aaf88..000b918f 100644 --- a/src/spatial/util.jl +++ b/src/spatial/util.jl @@ -93,7 +93,7 @@ function rotation_vector_rate(rotation_vector::AbstractVector{T}, angular_veloci @boundscheck length(ω) == 3 || error("ω has wrong length") θ = norm(ϕ) ϕ̇ = ω - if θ > eps(θ) + if θ > eps(typeof(θ)) s, c = sincos(θ) ϕ̇ += (ϕ × ω) / 2 + 1 / θ^2 * (1 - (θ * s) / (2 * (1 - c))) * ϕ × (ϕ × ω) end diff --git a/test/test_simulate.jl b/test/test_simulate.jl index 292a6320..f89f73a8 100644 --- a/test/test_simulate.jl +++ b/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 From fea0081050ba5074af7a522c5117cd09e152f919 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Fri, 19 Mar 2021 18:22:15 -0400 Subject: [PATCH 2/4] Fix derivative around zero. --- src/spatial/util.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/spatial/util.jl b/src/spatial/util.jl index 000b918f..c830fa93 100644 --- a/src/spatial/util.jl +++ b/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(typeof(θ)) s, c = sincos(θ) - ϕ̇ += (ϕ × ω) / 2 + 1 / θ^2 * (1 - (θ * s) / (2 * (1 - c))) * ϕ × (ϕ × ω) + ϕ̇ += 1 / θ^2 * (1 - (θ * s) / (2 * (1 - c))) * ϕ × (ϕ × ω) end ϕ̇ end From 865f3123fd4d4c6622011f3f9512f5473a9e9015 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Sat, 1 May 2021 09:42:04 -0400 Subject: [PATCH 3/4] Bump version for patch release. --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index ceb742bb..b4b1b0db 100644 --- a/Project.toml +++ b/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" From 2ea73b44ee44a7ab1039b820ce519dd76e87b106 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Sat, 1 May 2021 13:58:46 -0400 Subject: [PATCH 4/4] Add remove_subtree! --- src/RigidBodyDynamics.jl | 3 +- src/graphs/Graphs.jl | 1 + src/graphs/spanning_tree.jl | 25 +++++++++++++- src/mechanism_modification.jl | 35 ++++++++++++++++++++ test/test_graph.jl | 19 +++++++++++ test/test_mechanism_modification.jl | 51 +++++++++++++++++++++++++++++ 6 files changed, 132 insertions(+), 2 deletions(-) diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index 1365ee42..96ff7017 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -87,7 +87,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 diff --git a/src/graphs/Graphs.jl b/src/graphs/Graphs.jl index bf19eb4b..84a92239 100644 --- a/src/graphs/Graphs.jl +++ b/src/graphs/Graphs.jl @@ -39,6 +39,7 @@ export tree_index, ancestors, lowest_common_ancestor, + subtree_vertices, direction, directions diff --git a/src/graphs/spanning_tree.jl b/src/graphs/spanning_tree.jl index 2572f56e..b5728483 100644 --- a/src/graphs/spanning_tree.jl +++ b/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 diff --git a/src/mechanism_modification.jl b/src/mechanism_modification.jl index 2ac1767a..bd3b55c4 100644 --- a/src/mechanism_modification.jl +++ b/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) diff --git a/test/test_graph.jl b/test/test_graph.jl index b23e578d..1d8c6145 100644 --- a/test/test_graph.jl +++ b/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}}() diff --git a/test/test_mechanism_modification.jl b/test/test_mechanism_modification.jl index 000c2185..4c45a90a 100644 --- a/test/test_mechanism_modification.jl +++ b/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