Partially successful tests for frame angular velocity in RNE tests

Author: Larbino1

src/coordinates/coordinate_implementations.jl

@@ -489,6 +489,17 @@ _velocity(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = get_angular_vel(
 _jacobian(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = J_cache_view(cache, c)
 _acceleration(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = _get_angular_acc(cache, c.coord_data.frameID)
 
+function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:FrameAngularVelocity})
+    c = mc.coord_data
+    f_view = f_cache_view(cache, mc)
+    @assert length(f_view) == 3
+    f = f_cache_view(cache, mc)[SVector(1, 2, 3)] # Get the force applied to the coord as an SVector
+    fID = c.frameID
+    get_frame_torques(cache)[fID] += f
+    nothing
+end
+
+
 ####################################################################################################
 # CoordNorm
 ####################################################################################################

src/interface.jl

@@ -505,20 +505,20 @@ function _inverse_dynamics!(bundle::MechRNEBundle)
     # Backward pass
     foreach(get_force_components(m)) do component # add forces for each component to coordinates
         _add_opspace_force!(bundle, component)
-        @show f_cache_view(bundle, m[component.coord])
+        # @show f_cache_view(bundle, m[component.coord])
     end
-    @show frame_forces
-    println("Backward pass part 1")
+    # @show frame_forces
+    # println("Backward pass part 1")
     compute_in_reverse_coordinate_order(coordinates(m)) do coord # propagate forces through coordinates
         __propagate_opspace_force!(bundle, coord)
-        @show frame_forces
+        # @show frame_forces
     end
-    println("Backward pass part 2")
+    # println("Backward pass part 2")
     for (parent, child) in Iterators.reverse(m.rbtree.walk) # propagate forces through frames/joints
         frame_forces[parent] += frame_forces[child]
         δ = origin(get_transform(cache, CompiledFrameID(parent))) - origin(get_transform(cache, CompiledFrameID(child)))
         frame_torques[parent] += frame_torques[child] - cross(δ, frame_forces[child])
-        @show frame_forces
+        # @show frame_forces
     end
     # Project forces to joints
     foreach(joints(m)) do joint

test/inverse_dynamics_test.jl

@@ -1,15 +1,17 @@
-function test_inverse_dynamics(m)
+function test_inverse_dynamics(m; N=20)
     rng = MersenneTwister(1234)
 
-    # q = rand!(rng, zero_q(Float64, m))
-    # q̇ = rand!(rng, zero_q̇(Float64, m))
-    # u = rand!(rng, zero_u(Float64, m))
-    # u = zero_u(Float64, m)
-    q = [0., π/2]
-    q̇ = [0., 0.]
-    u = [1., 0.]
-    time = 0.0 # Time is irrelevant to these tests as no `TimeFuncJoint`s
-    gravity = SVector(0.0, 0.0, -10.)
+    for i = 1:N
+        t = 0.0
+        q = rand!(rng, zero_q(Float64, m))
+        q̇ = rand!(rng, zero_q̇(Float64, m))
+        gravity = randn(rng, SVector{3, Float64})
+        _test_inverse_dynamics(m, t, q, q̇, gravity)
+    end
+end
+
+function _test_inverse_dynamics(m, time, q, q̇, gravity)
+    u = zero_u(Float64, m)
 
     icache = new_inverse_dynamics_cache(m)
     dcache = new_dynamics_cache(m)
@@ -18,8 +20,8 @@ function test_inverse_dynamics(m)
     q̈ = get_q̈(dcache)
     inverse_dynamics!(icache, time, q, q̇, q̈, gravity)
 
-    @show get_u(icache)
-    @show u
+    # u is the control input used to compute forward dynamics, and is correct
+    # get_u(icache) is the result of the inverse dynamics computation, and should be equal to u
     @test get_u(icache) ≈ u atol=1e-7 rtol=1e-7
 
     # @show icache.cache.frame_cache.forces[6] - icache.cache.frame_cache.rbstates[6].acceleration.linear

test/runtests.jl

@@ -143,7 +143,7 @@ results = @testset "All tests" begin
     results = @testset "Velocity Kinematics AD" test_on_mechanisms(test_velocity_kinematics_vs_autodiff, small_immut_mechanisms);
     results = @testset "Coordinate tests" test_on_mechanisms(test_coordinates, compiled_systems);
     results = @testset "Dynamics tests" test_on_mechanisms(test_dynamics, systems_with_inertances);
-    # results = @testset "Inverse Dynamics tests" test_on_mechanisms(test_inverse_dynamics, compiled_mechanisms); # TODO 
+    results = @testset "Inverse Dynamics tests" test_on_mechanisms(test_inverse_dynamics, compiled_mechanisms); # TODO 
     results = @testset "Energy tests" test_on_mechanisms(test_energy, systems_with_inertances);
     results = @testset "RSON tests" test_on_mechanisms(test_rson, systems);
     # TODO ForwardDiff compat tests