Reproduce Acausal Example with 3D Pendulum

[traversaro]

Similarly to https://github.com/traversaro/MechanicsMTKExperiments/issues/1, I would like to reproduce with ModellingToolkit.jl the following Modelica model:

Model:

model PendulumExample
  inner Modelica.Mechanics.MultiBody.World world(label2 = "z", n = {0, 0, -1})  annotation(
    Placement(visible = true, transformation(origin = {-50, 10}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
  Modelica.Mechanics.MultiBody.Joints.Revolute revolute(n = {0, 1, 0}, phi(displayUnit = "rad"))  annotation(
    Placement(visible = true, transformation(origin = {-10, 10}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
  Modelica.Mechanics.MultiBody.Parts.Body body(m=1.0, r_CM={0.5,0,0}) annotation(
    Placement(visible = true, transformation(origin = {30, 10}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
equation
  connect(world.frame_b, revolute.frame_a) annotation(
    Line(points = {{-40, 10}, {-20, 10}}, color = {95, 95, 95}));
  connect(revolute.frame_b, body.frame_a) annotation(
    Line(points = {{0, 10}, {20, 10}}, color = {95, 95, 95}));

annotation(
    uses(Modelica(version = "4.0.0")),
    experiment(StopTime=5));
end PendulumExample;

Results:

[traversaro]

While working on this, I sketched something as in the following:

using ModelingToolkit, Plots, DifferentialEquations

@variables t

# Inspirted Modelica.Mechanics.MultiBody.Interfaces.Frame
@connector function MultiBody_Frame(;name)
    sts = @variables r_0[1:3](t) R[1:3, 1:3](t) force[1:3](t) [connect = Flow] torque[1:3](t) [connect = Flow]
    ODESystem(Equation[], t, sts, []; name=name)
end

function MultiBody_World(;name)
    @named frame = MultiBody_Frame()
    ps = []
    eqs = [
        frame.r_0 ~ zeros(3,1)
        frame.R ~ I(3)
    ]
    compose(ODESystem(eqs, t, [], ps; name=name), frame)
end

@connector function MultiBody_TwoFrames(;name)
    @named frame_a = Frame()
    @named frame_b = Frame()
    compose(ODESystem([], t, [], []; name=name), frame_a, frame_b)
end

@connector function MultiBody_TwoFramesRigid(;name)
    @named frame_a = Frame()
    @named frame_b = Frame()
    eqs = [
        frame_a.r_0 ~ frame_b.r_0
        frame_a.R ~ frame_b.R
       ]
    compose(ODESystem(eqs, t, [], []; name=name), frame_a, frame_b)
end

function vector_to_skew_symmetric(v)
    return [zero(T) -v[3] v[2];
            v[3] zero(T) -v[1];
            -v[2] v[1] zero(T)]
end

# Inspired from Modelica.Mechanics.Translational.Components.Mass
function MultiBody_Body(;name, m = 1.0)
    @named twoframes = MultiBody_TwoFramesRigid()
    @unpack frame_a, frame_b = twoframes
    sts = @variables r_0[1:3](t) v_0[1:3](t) a_0[1:3](t) R[1:3, 1:3](t) w_a[1:3](t) z_a[1:3](t)
    ps = @parameters m=m
    D = Differential(t)
    # Not Newton-Euler real equations, just a bunch of simplified ones to play around
    eqs = [
           r_0 ~ frame_a.r_0
           R ~ frame_a.R
           # Linear
           D(r_0) ~ v_0
           D(v_0) ~ a_0
           m*a_0 ~ (frame_a.f + frame_b.f) 
           # Angular
           # R is ᵃRₑ
           # w_A is ᵃωₐ,ₑ
           D(R)*R' ~ vector_to_skew_symmetric(w_a)
           D(w_a) ~ a_0
           m*z_a ~ (frame_a.t + frame_b.t) 
          ]
    extend(ODESystem(eqs, t, sts, ps; name=name), twoframes)
end

@named world = MultiBody_World()
# @named revolute  = MultiBody_Joint_Revolute()
@named body = MultiBody_Body()

pendulum_eqs = [
          connect(mass.flange_b, spring.flange_a)
         ]

@named _pendulum_model = ODESystem(pendulum_eqs, t)
@named pendulum_model = compose(_pendulum_model,
                          [world, body])
sys = structural_simplify(pendulum_model)
u0 = []
prob = ODAEProblem(sys, u0, (0, 5.0))
sol = solve(prob, Tsit5())
plot(sol)

However, I was having a lot of problems, so I switched to some simpler example: build a scalar integrator, a vector integrator and a rotatio integrator (where the input is a 3D angular velocity). This is tracked in https://github.com/traversaro/MechanicsMTKExperiments/issues/3 .