From 7b3c58d4021d11b433f08d03f2a4ad16cb5fc790 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Fri, 4 Nov 2016 02:20:02 -0400 Subject: [PATCH] Rewrite relative_acceleration without calling geometric_jacobian. --- src/mechanism_algorithms.jl | 32 +++++++++++++++++++++------ src/tree.jl | 21 ++++++++++++++++++ test/test_mechanism_algorithms.jl | 36 +++++++++++++++---------------- test/test_tree.jl | 12 ++++++++++- 4 files changed, 75 insertions(+), 26 deletions(-) diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl index 89d28622..3353fdd5 100644 --- a/src/mechanism_algorithms.jl +++ b/src/mechanism_algorithms.jl @@ -101,12 +101,32 @@ function geometric_jacobian{X, M, C}(state::MechanismState{X, M, C}, path::Path{ hcat(motionSubspaces...) end -function relative_acceleration{X, M, V}(state::MechanismState{X, M}, body::RigidBody{M}, base::RigidBody{M}, v̇::AbstractVector{V}) - p = path(state.mechanism, base, body) - J = geometric_jacobian(state, p) - v̇path = vcat([v̇[state.mechanism.vRanges[joint]] for joint in p.edgeData]...) - bias = -bias_acceleration(state, base) + bias_acceleration(state, body) - SpatialAcceleration(J, v̇path) + bias +function acceleration_wrt_ancestor{X, M, C, V}(state::MechanismState{X, M, C}, + descendant::TreeVertex{RigidBody{M}, Joint{M}}, + ancestor::TreeVertex{RigidBody{M}, Joint{M}}, + v̇::AbstractVector{V}) + mechanism = state.mechanism + T = promote_type(C, V) + descendantFrame = default_frame(mechanism, vertex_data(descendant)) + accel = zero(SpatialAcceleration{T}, descendantFrame, descendantFrame, root_frame(mechanism)) + descendant == ancestor && return accel + + current = descendant + while current != ancestor + joint = edge_to_parent_data(current) + v̇joint = view(v̇, mechanism.vRanges[joint]) + jointAccel = SpatialAcceleration(motion_subspace(state, joint), v̇joint) + accel = jointAccel + accel + current = parent(current) + end + -bias_acceleration(state, vertex_data(ancestor)) + bias_acceleration(state, vertex_data(descendant)) + accel +end + +function relative_acceleration(state::MechanismState, body::RigidBody, base::RigidBody, v̇::AbstractVector) + bodyVertex = findfirst(tree(state.mechanism), body) + baseVertex = findfirst(tree(state.mechanism), base) + lca = lowest_common_ancestor(baseVertex, bodyVertex) + -acceleration_wrt_ancestor(state, baseVertex, lca, v̇) + acceleration_wrt_ancestor(state, bodyVertex, lca, v̇) end kinetic_energy{X, M}(state::MechanismState{X, M}, body::RigidBody{M}) = kinetic_energy(spatial_inertia(state, body), twist_wrt_world(state, body)) diff --git a/src/tree.jl b/src/tree.jl index 2774bc0a..d798e9d9 100644 --- a/src/tree.jl +++ b/src/tree.jl @@ -13,7 +13,9 @@ export isleaf, toposort, detach!, + isancestor, ancestors, + lowest_common_ancestor, leaves, path, insert_subtree!, @@ -137,6 +139,25 @@ function copy{V, E}(v::TreeVertex{V, E}) ret end +function isancestor{V, E}(vertex::TreeVertex{V, E}, candidate::TreeVertex{V, E}) + current = vertex + while !isroot(current) + current = parent(current) + current == candidate && return true + end + return false +end + +function lowest_common_ancestor{V, E}(a::TreeVertex{V, E}, b::TreeVertex{V, E}) + (a == b || isroot(b)) && return b + current = a + while !isancestor(b, current) + isroot(current) && error("vertices are not part of the same tree") + current = parent(current) + end + current +end + function ancestors{V, E}(vertex::TreeVertex{V, E}) current = vertex result = Vector{TreeVertex{V, E}}() diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl index ad8e9435..d88e0561 100644 --- a/test/test_mechanism_algorithms.jl +++ b/test/test_mechanism_algorithms.jl @@ -77,25 +77,23 @@ end @testset "relative_acceleration" begin - bs = Set(bodies(mechanism)) - body = rand([bs...]) - delete!(bs, body) - base = rand([bs...]) - v̇ = rand(num_velocities(mechanism)) - Ṫ = relative_acceleration(x, body, base, v̇) - - q = configuration_vector(x) - v = velocity_vector(x) - q̇ = configuration_derivative(x) - q_autodiff = create_autodiff(q, q̇) - v_autodiff = create_autodiff(v, v̇) - x_autodiff = MechanismState(eltype(q_autodiff), mechanism) - set_configuration!(x_autodiff, q_autodiff) - set_velocity!(x_autodiff, v_autodiff) - twist_autodiff = relative_twist(x_autodiff, body, base) - accel_vec = [ForwardDiff.partials(x, 1)::Float64 for x in (Array(twist_autodiff))] - - @test isapprox(Array(Ṫ), accel_vec; atol = 1e-12) + for body in bodies(mechanism) + for base in bodies(mechanism) + v̇ = rand(num_velocities(mechanism)) + Ṫ = relative_acceleration(x, body, base, v̇) + q = configuration_vector(x) + v = velocity_vector(x) + q̇ = configuration_derivative(x) + q_autodiff = create_autodiff(q, q̇) + v_autodiff = create_autodiff(v, v̇) + x_autodiff = MechanismState(eltype(q_autodiff), mechanism) + set_configuration!(x_autodiff, q_autodiff) + set_velocity!(x_autodiff, v_autodiff) + twist_autodiff = relative_twist(x_autodiff, body, base) + accel_vec = [ForwardDiff.partials(x, 1)::Float64 for x in (Array(twist_autodiff))] + @test isapprox(Array(Ṫ), accel_vec; atol = 1e-12) + end + end end @testset "motion subspace / twist wrt world" begin diff --git a/test/test_tree.jl b/test/test_tree.jl index d18d0c27..d1872b05 100644 --- a/test/test_tree.jl +++ b/test/test_tree.jl @@ -31,7 +31,17 @@ sub = subtree(v4) @test isroot(sub) @test length(toposort(sub)) == 5 - # println(sub) + end + + @testset "ancestors" begin + for vertex in toposort(tree) + for ancestor in ancestors(vertex) + @test isancestor(vertex, ancestor) + end + for self_or_descendant in toposort(subtree(vertex)) + @test !isancestor(vertex, self_or_descendant) + end + end end @testset "reroot" begin