Renamed panda mechanisms, split tests into runtests_manual.jl and setup_test_mechanisms.jl to make running tests independently simpler. Fixed projecting joint torque for revolute joints.

Author: Larbino1

RSONs/rsons/franka_panda/pandaDrill.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_drill",
     "frames": [
         "root_frame",
         "L1_frame",

RSONs/rsons/franka_panda/pandaDummyInstrument.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_dummy_instrument",
     "frames": [
         "root_frame",
         "L1_frame",

RSONs/rsons/franka_panda/pandaSurgical.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_surgical",
     "frames": [
         "root_frame",
         "L1_frame",

RSONs/rsons/franka_panda/pandaSurgicalStryker.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_surgical_stryker",
     "frames": [
         "root_frame",
         "L1_frame",

RSONs/rsons/franka_panda/pandaSurgicalV2.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_surgical_v2",
     "frames": [
         "root_frame",
         "L1_frame",

RSONs/rsons/franka_panda/pandaVeterinarian.rson

@@ -1,7 +1,7 @@
 {
     "rson_version": "0.1.0",
     "type": "mechanism",
-    "name": "panda",
+    "name": "panda_veterinarian",
     "frames": [
         "root_frame",
         "L1_frame",

examples/rne_visualization.jl

@@ -5,54 +5,87 @@ using
     LinearAlgebra,
     StaticArrays,
     VMRobotControl
-
-X = SVector{3, Float64}(1.0, 0.0, 0.0)
-Y = SVector{3, Float64}(0.0, 1.0, 0.0)
-Z = SVector{3, Float64}(0.0, 0.0, 1.0)
-
-T1 = zero(Transform{Float64})
-J1 = Revolute(Y, T1)
-
-T2 = Transform(Z)
-J2 = Revolute(Y, T2)
-
-T3 = Transform(Z)
-J3 = Rigid(T3)
-
-mech = Mechanism{Float64}("2Link")
-cart_frame = add_frame!(mech, "Cart")
-L2_frame = add_frame!(mech, "L2")
-EE_frame = add_frame!(mech, "EE")
-
-add_joint!(mech, J1; parent="root_frame",   child=cart_frame,               id="J1")
-add_joint!(mech, J2; parent=cart_frame,     child=L2_frame,                 id="J2")
-add_joint!(mech, J3, parent=L2_frame,       child=EE_frame;                 id="J3")
-
-# Then, we add a coordinate to the mechanism to represent the tip of the pendulum, and a
-# coordinate to represent the position of the cart. These are used to add point masses to
-# the mechanism.
-add_coordinate!(mech, FrameOrigin(EE_frame);                                id="tip_pos")
-add_coordinate!(mech, FrameOrigin(cart_frame);                              id="cart_pos")
-add_component!(mech, PointMass(1.0, "cart_pos");                            id="cart_mass")
-add_component!(mech, PointMass(1.0, "tip_pos");                             id="pendulum_mass")
-
+# begin
+#     X = SVector{3, Float64}(1.0, 0.0, 0.0)
+#     Y = SVector{3, Float64}(0.0, 1.0, 0.0)
+#     Z = SVector{3, Float64}(0.0, 0.0, 1.0)
+
+#     T1 = zero(Transform{Float64})
+#     J1 = Revolute(Y, T1)
+
+#     T2 = Transform(Z)
+#     J2 = Revolute(Y, T2)
+
+#     T3 = Transform(Z)
+#     J3 = Rigid(T3)
+
+#     mech = Mechanism{Float64}("2Link")
+#     cart_frame = add_frame!(mech, "Cart")
+#     L2_frame = add_frame!(mech, "L2")
+#     EE_frame = add_frame!(mech, "EE")
+
+#     add_joint!(mech, J1; parent="root_frame",   child=cart_frame,               id="J1")
+#     add_joint!(mech, J2; parent=cart_frame,     child=L2_frame,                 id="J2")
+#     add_joint!(mech, J3, parent=L2_frame,       child=EE_frame;                 id="J3")
+
+#     # Then, we add a coordinate to the mechanism to represent the tip of the pendulum, and a
+#     # coordinate to represent the position of the cart. These are used to add point masses to
+#     # the mechanism.
+#     add_coordinate!(mech, FrameOrigin(EE_frame);                                id="tip_pos")
+#     add_coordinate!(mech, FrameOrigin(cart_frame);                              id="cart_pos")
+#     add_component!(mech, PointMass(1.0, "cart_pos");                            id="cart_mass")
+#     add_component!(mech, PointMass(1.0, "tip_pos");                             id="pendulum_mass")
+#     m = compile(mech)
+# end
+    
 # We compile the mechanism, and setup an ODE problem to simulate the dynamics of the rail robot.
-m = compile(mech)
+m = compiled_mechanisms[end]
 cache = Observable(new_inverse_dynamics_cache(m))
+# @testset test_inverse_dynamics(m)
 
+bundle = cache[]
 
 begin
+
+
 t = 0.0
 q = [0.0, π/2]
-q̇ = zero_q̇(m)
-q̈ = zero_q̈(m)
-g = SVector{3, Float64}(0.0, 0.0, -9.81)
-inverse_dynamics!(cache[], t, q, q̇, q̈, g)
+q = zero_q(m) + randn(7)
+q̇ = zero_q̇(m) + randn(7)
+g = SVector{3, Float64}(0.0, 0.0, -10) #+ rand(SVector{3, Float64})
+
+# rng = MersenneTwister(1234)
+# t = 0.0
+# q = rand!(rng, zero_q(Float64, m))
+# q̇ = rand!(rng, zero_q̇(Float64, m))
+# g = randn(rng, SVector{3, Float64})
+
+u = zero_u(m)
+
+dcache = new_dynamics_cache(m)
+dynamics!(dcache, t, q, q̇, g, u)
+q̈ = get_q̈(dcache)
+
+VMRobotControl._inverse_dynamics_set_inputs!(bundle, t, q, q̇, q̈, g)
+VMRobotControl._inverse_dynamics_forward_pass!(bundle)
+VMRobotControl._inverse_dynamics_zero!(bundle)
+VMRobotControl._inverse_dynamics_backward_pass_a!(bundle)
+VMRobotControl._inverse_dynamics_backward_pass_b!(bundle)
+VMRobotControl._inverse_dynamics_backward_pass_c!(bundle)
+VMRobotControl._inverse_dynamics_backward_pass_d!(bundle)
+
+# q̈_out = dynamics!(dcache, t, q, q̇, g, get_u(bundle))
+
+@test get_u(cache[]) ≈ u atol=1e-7 rtol=1e-7
+# @test q̈ ≈ q̈_out atol=1e-7 rtol=1e-7
+end
 
+
+begin
 fig = Figure()
 display(fig)
 ls = LScene(fig[1, 1]; show_axis=false)
-robotsketch!(ls, cache; linewidth=3)
+robotsketch!(ls, cache; linewidth=3, scale=0.05)
 
 frameIDs = get_compiled_frameID.((cache[],), frames(cache[]))
 tfs = map(cache) do cache
@@ -65,14 +98,18 @@ fs = map(cache) do cache
     fs = map(id -> VMRobotControl.get_frame_force(cache, id), frameIDs)
 end
 f_scale = map(fs) do fs
-    0.1/norm(maximum(fs))
+    0.2/maximum(norm(fs))
 end
 τs = map(cache) do cache
     map(id -> VMRobotControl.get_frame_torque(cache, id), frameIDs)
 end
+τ_scale = map(τs) do τs
+    0.4/maximum(norm(τs))
+end
+
 
-arrows!(ls, positions, fs; lengthscale=f_scale, arrowsize=0.1, color=:red, )
-arrows!(ls, positions, τs; lengthscale=f_scale, arrowsize=0.1, color=:red, )
+arrows!(ls, positions, fs; lengthscale=f_scale, arrowsize=map(f->*(f, 50), f_scale), color=:red, )
+arrows!(ls, positions, τs; lengthscale=τ_scale, arrowsize=0.04, color=:blue, )
 
 
 end

src/components/components.jl

@@ -359,10 +359,8 @@ Add opspace force `f` to the cache for coordinate `c`.
 @inline function _add_opspace_force!(bundle::CacheBundle, c::C) where C<:ComponentData
     @assert hasfield(C, :coord) "Component '$c' does not have a field 'coord'. If this is a valid component, please implement `_add_opspace_force!` for it."
     f::SVector = opspace_force(bundle, c)
-    coord_id = c.coord
-    coord = bundle[coord_id]
-    f_cache_view(bundle, coord) .-= f # f is negated, due to the definition of bias forces. e.g. If a spring opspace force is Kz
-    nothing
+    f_view = f_cache_view(bundle, bundle[c.coord])
+    f_view .-= f # f is negated, due to the definition of bias forces. e.g. If a spring opspace force is Kz
 end
 
 function _opspace_force(cache::CacheBundle, c::ComponentData)

src/coordinates/coordinate_implementations.jl

@@ -493,9 +493,9 @@ function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:FrameAngularVe
     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
+    f = f_cache_view(cache, mc)[SVector(1, 2, 3)] # Get the applied torque as an SVector
     fID = c.frameID
-    get_frame_torques(cache)[fID] += f
+    get_frame_torques(cache)[fID] += f # Apply torque to the frame
     nothing
 end
 

src/interface.jl

@@ -481,69 +481,94 @@ end
 ===============================================================================#
 function inverse_dynamics! end
 
-function _inverse_dynamics!(bundle::MechRNEBundle) 
-    # Forward pass
+function _inverse_dynamics!(bundle::MechRNEBundle)   
+    _inverse_dynamics_forward_pass!(bundle)
+    _inverse_dynamics_zero!(bundle)
+    _inverse_dynamics_backward_pass_a!(bundle)
+    _inverse_dynamics_backward_pass_b!(bundle)
+    _inverse_dynamics_backward_pass_c!(bundle)
+    _inverse_dynamics_backward_pass_d!(bundle)
+    nothing
+end
+
+function _inverse_dynamics_set_inputs!(bundle, t, q, q̇, q̈, gravity)!
+    t_cache, q_cache, q̇_cache, q̈_cache, g_cache, u_cache, = get_t(bundle), get_q(bundle), get_q̇(bundle), get_q̈(bundle), get_gravity(bundle), get_u(bundle)
+
+    t_cache[] = t
+    copyto!(q_cache, q)
+    copyto!(q̇_cache, q̇)
+    copyto!(q̈_cache, q̈)
+    g_cache[] = gravity
+end
+
+function _inverse_dynamics_forward_pass!(bundle)
     _acceleration_kinematics!(bundle)
     m, cache = bundle.mechanism, bundle.cache
     compute_in_coordinate_order(coordinates(m)) do coord
         __configuration!(bundle, coord)
         __velocity!(bundle, coord)
         __acceleration!(bundle, coord)
     end
-    
-    #
-    u = get_u(bundle)
-    frame_forces = get_frame_forces(bundle)
-    frame_torques = get_frame_torques(bundle)
+    nothing
+end
 
+function _inverse_dynamics_zero!(bundle)
+    u = get_u(bundle)
+    frame_forces, frame_torques = get_frame_forces(bundle), get_frame_torques(bundle)   
     # Zero all coordinate/frame forces and torques.
     fill!(bundle.cache.coord_cache.f, zero(eltype(bundle.cache.coord_cache.f)))
     fill!(frame_forces, zero(eltype(frame_forces)))
     fill!(frame_torques, zero(eltype(frame_torques)))
     fill!(u, zero(eltype(u)))
+    nothing
+end
 
-    # Backward pass
-    foreach(get_force_components(m)) do component # add forces for each component to coordinates
+function _inverse_dynamics_backward_pass_a!(bundle)
+    # Add forces for each component to coordinates
+    foreach(get_force_components(bundle.mechanism)) do component 
         _add_opspace_force!(bundle, component)
-        # @show f_cache_view(bundle, m[component.coord])
     end
-    # @show frame_forces
-    # println("Backward pass part 1")
-    compute_in_reverse_coordinate_order(coordinates(m)) do coord # propagate forces through coordinates
+    # Now the force for each coordinate with a component *directly* attached to it is set
+    nothing
+end
+
+function _inverse_dynamics_backward_pass_b!(bundle)
+    # Propagate forces through coordinates.
+    compute_in_reverse_coordinate_order(coordinates(bundle.mechanism)) do coord 
         __propagate_opspace_force!(bundle, coord)
-        # @show frame_forces
     end
-    # println("Backward pass part 2")
-    for (parent, child) in Iterators.reverse(m.rbtree.walk) # propagate forces through frames/joints
+    # Now, *frame* forces/torques contain the force/torque applied to each frame by the components
+    nothing
+end
+
+function _inverse_dynamics_backward_pass_c!(bundle)
+    frame_forces, frame_torques = get_frame_forces(bundle), get_frame_torques(bundle)   
+    # propagate forces through frames/joints
+    for (parent, child) in Iterators.reverse(bundle.mechanism.rbtree.walk) # TODO check this order is correct... might not work
         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
+        δ = origin(get_transform(bundle, CompiledFrameID(child))) - origin(get_transform(bundle, CompiledFrameID(parent)))
+        frame_torques[parent] += frame_torques[child] + cross(δ, frame_forces[child])
     end
+end
+
+function _inverse_dynamics_backward_pass_d!(bundle)
+    u = get_u(bundle)
+    frame_forces, frame_torques = get_frame_forces(bundle), get_frame_torques(bundle)   
     # Project forces to joints
-    foreach(joints(m)) do joint
+    foreach(joints(bundle.mechanism)) do joint
         length(q_idxs(joint)) == 0 && return nothing
         p, c = joint.parentFrameID, joint.childFrameID
-        tf_p, tf_c = get_transform(cache, p), get_transform(cache, c)
+        tf_p, tf_c = get_transform(bundle, p), get_transform(bundle, c)
         f_p, τ_p, f_c, τ_c = frame_forces[p], frame_torques[p], frame_forces[c], frame_torques[c]
         u[q_idxs(joint)] .= _project_forces_to_jointspace(joint.jointData, tf_p, tf_c, f_p, f_c, τ_p, τ_c)
         nothing
     end
-    nothing
 end
 
 function inverse_dynamics!(bundle::MechRNEBundle, t, q, q̇, q̈, gravity)
-    t_cache, q_cache, q̇_cache, q̈_cache, g_cache, u_cache, = 
-        get_t(bundle), get_q(bundle), get_q̇(bundle), get_q̈(bundle), get_gravity(bundle), get_u(bundle)
-
-    t_cache[] = t
-    copyto!(q_cache, q)
-    copyto!(q̇_cache, q̇)
-    copyto!(q̈_cache, q̈)
-    g_cache[] = gravity
+    _inverse_dynamics_set_inputs!(bundle, t, q, q̇, q̈, gravity)
     _inverse_dynamics!(bundle)
-
-    u_cache
+    get_u(bundle)
 end