Merge pull request #113 from tkoolen/improve-relative-acceleration

Rewrite relative_acceleration so that it doesn't call geometric_jacobian.

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

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

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

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