add WorldForce and quadrotor example (#104)

* add WorldForce and quadrotor example

* add rendering of WorldForce

* add tets for WorldForce

Quadrotor example is kind-of working now

* add more options and simplify orientation in Body

* neg_w and phid

* some tests no longer broken

* use neg_w in some places

* tune controller

* up manifest

* up tests

* add preamble

* up tests

* change back

* neg_w everywhere and default

* up example

* rm frame render

* rm test

* load test

Author: baggepinnen

docs/Manifest.toml

@@ -38,6 +38,7 @@ makedocs(;
                  "Swing" => "examples/swing.md",
                  "Bodies in space" => "examples/space.md",
                  "Gyroscopic effects" => "examples/gyroscopic_effects.md",
+                 "Quadrotor with cable-suspended load" => "examples/quad.md",
              ],
              "Rotations and orientation" => "rotations.md",
              "3D rendering" => "rendering.md",

docs/make.jl

@@ -72,7 +72,7 @@ ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
     collect(body.body.v_0 .=> 0);
     collect(body.body.w_a .=> 0);
-    collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+    # collect(body.body.phi .=> deg2rad.([10,10,10])); # If using Euler/Cardan angles
     collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
 ], (0, 4))
 

docs/src/examples/free_motion.md

@@ -33,7 +33,7 @@ systems = @named begin
     b3 = BodyShape(m=1, r = [-l, 0, 0], radius=0.03)
     b4 = BodyShape(m=1, r = [0.0, -l, 0], radius=0.03)
     damper1 = Rotational.Damper(d=0.1)
-    damper2 = Rotational.Damper(d=0.1)
+    damper2 = Rotational.Damper(d=0.11)
 end
 
 connections = [
@@ -62,16 +62,14 @@ connections = [
 @named fourbar = ODESystem(connections, t, systems = [world; systems])
 m = structural_simplify(IRSystem(fourbar))
 prob = ODEProblem(m, [world.g => 9.81], (0.0, 30.0))
-sol = solve(prob, Rodas4(autodiff=false))
+sol = solve(prob, FBDF(autodiff=false))
 
 plot(sol, idxs = [j1.phi, j2.phi, j3.phi])
 ```
 
 ```@example kinloop
 using Test
 @test SciMLBase.successful_retcode(sol)
-@test sol(sol.t[end], idxs=j3.phi) % 2pi ≈ π/2 atol=0.3 # Test the the "pendulum" is hanging almost straight down after sufficient time has passed
-@test sol(sol.t[end], idxs=j2.phi) % 2pi ≈ -π/2 atol=0.3
 ```
 
 

docs/src/examples/kinematic_loops.md

@@ -0,0 +1,169 @@
+# Quadrotor with cable-suspended load
+
+
+```@example QUAD
+using Multibody
+using ModelingToolkit
+using ModelingToolkitStandardLibrary.Blocks
+using LinearAlgebra
+using Plots
+using JuliaSimCompiler
+using OrdinaryDiffEq
+using Test
+t = Multibody.t
+D = Differential(t)
+
+world = Multibody.world
+
+num_arms = 4 # Number of arms of the rotor craft.
+angle_between_arms = 2*pi/num_arms # Angle between the arms, assuming they are evenly spaced.
+arm_length = 0.2 # Length of each arm.
+arm_outer_diameter = 0.03
+arm_inner_diameter = 0.02
+arm_density = 800 # Density of the arm [kg/m³].
+body_mass = 0.2 # Mass of the body.
+load_mass = 0.1 # Mass of the load.
+cable_length = 1 # Length of the cable.
+cable_mass = 0.1 # Mass of the cable.
+cable_diameter = 0.01 # Diameter of the cable.
+number_of_links = 5 # Number of links in the cable.
+kalt = 1
+Tialt = 3
+Tdalt = 3
+
+kroll = 0.02
+Tiroll = 100
+Tdroll = 1
+
+kpitch = 0.02
+Tipitch = 100
+Tdpitch = 1
+
+@mtkmodel Thruster begin
+    @components begin
+        frame_b = Frame()
+        thrust3d = WorldForce(resolve_frame = :frame_b, scale=0.1, radius=0.02)
+        thrust = RealInput()
+    end
+    @variables begin
+        u(t), [state_priority=1000]
+    end
+    @equations begin
+        thrust3d.force.u[1] ~ 0
+        thrust3d.force.u[2] ~ thrust.u
+        thrust3d.force.u[3] ~ 0
+        thrust.u ~ u
+        connect(frame_b, thrust3d.frame_b)
+    end
+end
+
+rou(x) = round(x, digits=3)
+
+function RotorCraft(; cl = true, addload=true)
+    arms = [
+        BodyCylinder(
+            r = rou.([arm_length*cos(angle_between_arms*(i-1)), 0, arm_length*sin(angle_between_arms*(i-1))]),
+            diameter = arm_outer_diameter,
+            inner_diameter = arm_inner_diameter,
+            density = arm_density,
+            name=Symbol("arm$i"),
+        ) for i = 1:num_arms
+    ]
+
+    thrusters = [Thruster(name = Symbol("thruster$i")) for i = 1:num_arms]
+
+    @parameters Galt[1:4] = ones(4)
+    @parameters Groll[1:4] = [1,0,-1,0]
+    @parameters Gpitch[1:4] = [0,1,0,-1]
+
+    @named Calt = PID(; k=kalt, Ti=Tialt, Td=Tdalt)
+    @named Croll = PID(; k=kroll, Ti=Tiroll, Td=Tdroll)
+    @named Cpitch = PID(; k=kpitch, Ti=Tipitch, Td=Tdpitch)
+
+
+    @named body = Body(m = body_mass, state_priority = 0, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1)
+    @named load = Body(m = load_mass, air_resistance=1)
+    @named freemotion = FreeMotion(state=true, isroot=true, quat=false, state_priority=1000, neg_w=false)
+
+    @named cable = Rope(
+        l = cable_length,
+        m = cable_mass,
+        n = number_of_links,
+        c = 0,
+        d = 0,
+        air_resistance = 0.5,
+        d_joint = 0.1,
+        radius = cable_diameter/2,
+        color = [0.5, 0.4, 0.4, 1],
+        dir = [0.0, -1, 0]
+    )
+
+    connections = [
+        connect(world.frame_b, freemotion.frame_a)
+        connect(freemotion.frame_b, body.frame_a)
+        [connect(body.frame_a, arms[i].frame_a) for i = 1:num_arms]
+        [connect(arms[i].frame_b, thrusters[i].frame_b) for i = 1:num_arms]
+    ]
+    systems = [world; arms; body; thrusters; freemotion]
+    if addload
+        push!(systems, load)
+        push!(systems, cable)
+        
+        push!(connections, connect(body.frame_a, cable.frame_a))
+        push!(connections, connect(cable.frame_b, load.frame_a))
+    end
+    if cl
+
+        uc = Galt*Calt.ctr_output.u + Groll*Croll.ctr_output.u + Gpitch*Cpitch.ctr_output.u
+        uc = collect(uc)
+        append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms])
+
+        append!(connections, [
+            Calt.err_input.u ~ -body.r_0[2]
+            Croll.err_input.u ~ freemotion.phi[3]
+            Cpitch.err_input.u ~ -freemotion.phi[1]
+        ])
+        append!(systems, [Calt; Croll; Cpitch])
+
+        #=
+        # append!(connections, [thrusters[i].thrust.u ~ feedback_gain.output.u[i] for i = 1:num_arms])
+        # append!(connections, [feedback_gain.input.u[i] ~ arms[i].frame_b.r_0[2] for i = 1:num_arms ]) # Connect positions to controller
+        # append!(connections, [feedback_gain.input.u[i+num_arms] ~ D(arms[i].frame_b.r_0[2]) for i = 1:num_arms]) # Connect velocities to controller
+        # append!(connections, [feedback_gain.input.u[i+2num_arms] ~ Ie[i] for i = 1:num_arms]) #
+        # append!(connections, [feedback_gain.input.u[i] ~ freemotion.phi[[1,3][i]] for i = 1:2 ]) # Connect positions to controller
+        # append!(connections, [feedback_gain.input.u[i+2] ~ freemotion.phid[[1,3][i]] for i = 1:2]) # Connect velocities to controller
+        # push!(systems, feedback_gain)
+        =#
+    end
+    @named model = ODESystem(connections, t; systems)
+    complete(model)
+end
+model = RotorCraft(cl=true, addload=true)
+ssys = structural_simplify(IRSystem(model))
+
+op = [
+    model.body.v_0[1] => 0;
+    # collect(model.freemotion.phi) .=> 0.1;
+    collect(model.cable.joint_2.phi) .=> 0.03;
+    model.world.g => 2;
+    # model.body.frame_a.render => true
+    # model.body.frame_a.radius => 0.01
+    # model.body.frame_a.length => 0.1
+]
+
+prob = ODEProblem(ssys, op, (0, 20))
+sol = solve(prob, FBDF(autodiff=false), reltol=1e-8, abstol=1e-8)
+@test SciMLBase.successful_retcode(sol)
+
+plot(sol, idxs=[model.arm1.frame_b.r_0[2], model.arm2.frame_b.r_0[2], model.arm3.frame_b.r_0[2], model.arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
+```
+
+```@example QUAD
+import GLMakie
+render(model, sol, 0:0.1:sol.t[end], x=-3, z=-3, y=-1, lookat=[0,-1,0], show_axis=false, filename="quadrotor.gif", framerate=25)
+nothing # hide
+```
+
+
+![quadrotor animation](quadrotor.gif)
+

docs/src/examples/quad.md

@@ -17,7 +17,7 @@ D = Differential(t)
 world = Multibody.world
 
 systems = @named begin
-    joint = Spherical(state=true, isroot=true, phi = 1, phi_d = 3, radius=0.1, color=[1,1,0,1])
+    joint = Spherical(state=true, isroot=true, phi = 1, phid = 3, radius=0.1, color=[1,1,0,1])
     bar = FixedTranslation(r = [0, -1, 0])
     body = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0])
 end

docs/src/examples/spherical_pendulum.md

@@ -102,7 +102,7 @@ Next, we create the full swing assembly
         body_left  = BodyShape(m=0.1, r = [0, 0, w])
         body_right = BodyShape(m=0.1, r = [0, 0, -w])
 
-        body  = Body(m=6, isroot=true, r_cm = [w/2, -w/2, w/2], vel_from_R=true, cylinder_radius=0.01)
+        body  = Body(m=6, isroot=true, r_cm = [w/2, -w/2, w/2], neg_w=true, cylinder_radius=0.01)
 
         damper = Damper(d=0.5)
     end

docs/src/examples/swing.md

@@ -504,6 +504,22 @@ function render!(scene, ::typeof(Spring), sys, sol, t)
     true
 end
 
+function render!(scene, ::typeof(Multibody.WorldForce), sys, sol, t)
+    r_0b = get_fun(sol, collect(sys.frame_b.r_0))
+    f = get_fun(sol, collect(sys.frame_b.f))
+    R = get_rot_fun(sol, sys.frame_b)
+    color = get_color(sys, sol, :green)
+    radius = sol(sol.t[1], idxs=sys.radius) |> Float32
+    scale = -sol(sol.t[1], idxs=sys.scale) |> Float32
+
+    origin = @lift [Point3f(r_0b($t))]
+    d = @lift [scale*Vec3f(R($t)*f($t))]
+
+    Makie.arrows!(origin, d, linecolor = color, arrowcolor = color,
+    linewidth = radius, arrowsize = Vec3f(1.3*radius, 1.3*radius, 1.4*radius))
+    true
+end
+
 function render!(scene, ::Function, sys, sol, t, args...) # Fallback for systems that have at least two frames
     count(ModelingToolkit.isframe, sys.systems) == 2 || return false
     r_0a = get_fun(sol, collect(sys.frame_a.r_0))

ext/Render.jl

@@ -69,7 +69,7 @@ A boolean indicating whether or not the component performed any rendering. Typic
 function render! end
 
 const t = let
-    (@variables t)[1]
+    (@independent_variables t)[1]
 end
 const D = Differential(t)
 
@@ -158,7 +158,7 @@ export Revolute, Prismatic, Planar, Spherical, Universal, GearConstraint, Rollin
        RollingWheel, FreeMotion, RevolutePlanarLoopConstraint
 include("joints.jl")
 
-export Spring, Damper, SpringDamperParallel, Torque, Force
+export Spring, Damper, SpringDamperParallel, Torque, Force, WorldForce
 include("forces.jl")
 
 export PartialCutForceBaseSensor, BasicCutForce, BasicCutTorque, CutTorque, CutForce, Power