External wrenches two ways

external_wrenches_two_ways.jl

using RigidBodyDynamics
using Test

function randmech()
    rand_tree_mechanism(Float64,
        QuaternionFloating{Float64},
        [Revolute{Float64} for i = 1 : 5]...,
        [Fixed{Float64} for i = 1 : 5]...,
        [Prismatic{Float64} for i = 1 : 5]...,
        [Planar{Float64} for i = 1 : 5]...,
        [SPQuatFloating{Float64} for i = 1:2]...,
        [SinCosRevolute{Float64} for i = 1:2]...
    )
end

mechanism = randmech()

state = MechanismState(mechanism)

worldframe = root_frame(mechanism)
external_wrenches = Dict(BodyID(b) => rand(Wrench{Float64}, worldframe) for b in bodies(mechanism))

c = dynamics_bias(state, external_wrenches)

c0 = dynamics_bias(state)

τ_external_wrenches = zeros(num_velocities(state))
world = root_body(mechanism)
for (bodyid, wrench) in external_wrenches
    J = geometric_jacobian(state, path(mechanism, findbody(mechanism, bodyid), world))
    global τ_external_wrenches += torque(J, wrench)
end

@test c ≈ c0 + τ_external_wrenches