JuliaComputing / Multibody.jl

Other
7 stars 0 forks source link

Mysterious negative sign required _sometimes_ #105

Open baggepinnen opened 2 months ago

baggepinnen commented 2 months ago

This test demonstrates a problem in our description of orientations

Two opposing forces (green arrows) are both acting on a body. Another identical body is connected to the first body.

image

Questions

@mtkmodel TestWorldForce begin
    @components begin
        world = W()
        forcea = WorldForce(resolve_frame=:frame_b)
        forceb = WorldForce(resolve_frame=:frame_b)
        b0 = Body(m=1, state_priority=0)
        body = Body(m=1, state=true, isroot=true, quat=true, neg_w=false)
    end
    @parameters begin
        f[1:3]
    end
    begin
        f = collect(f)
    end
    @equations begin
        connect(forcea.frame_b, body.frame_a, b0.frame_a)
        connect(forceb.frame_b, body.frame_a)
        forcea.force.u ~ f
        forceb.force.u ~ -f
    end
end

@named testwf = TestWorldForce()
testwf = complete(testwf)
ssys = structural_simplify(IRSystem(testwf))
prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [1,0,0]], (0, 1))
sol = solve(prob, Tsit5())
# plot(sol)
@test sol(1, idxs=testwf.body.r_0) ≈ [0, 0.0, 0.0] atol=1e-3