From 15ce572930a47953fd65496b7b74c954ef867ae8 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 30 Jul 2024 12:49:19 +0200 Subject: [PATCH 1/2] add text to quad tutorial --- docs/src/examples/quad.md | 71 ++++++++++++++++++++------------------- ext/Render.jl | 4 +-- 2 files changed, 39 insertions(+), 36 deletions(-) diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index 9137ec3d..edd0539f 100644 --- a/docs/src/examples/quad.md +++ b/docs/src/examples/quad.md @@ -1,5 +1,10 @@ # Quadrotor with cable-suspended load +![quadrotor animation](quadrotor.gif) + +This example builds a simple model of a quadrotor that carries a load suspended by a cable. The quadrotor has four arms, each with a thruster at the end. The quadrotor is controlled by three PID controllers: one for altitude, one for roll, and one for pitch (for simplicity, no position controller is included here). + +The main body of the aircraft is modeled using a [`Body`](@ref), and the arms are modeled using [`BodyCylinder`](@ref) components. The total inertia of the body and arms are automatically computed using the geometry and density properties of the bodies involved. The thrusters are modeled using a custom component called `Thruster` that applies a force in the y-direction. The thruster model is kinematical only, and does not model the rotation dynamics of the motors or any aerodynamics. The quadrotor is connected to the world frame using a [`FreeMotion`](@ref) joint, which allows the quadrotor to move freely in space. ```@example QUAD using Multibody @@ -27,6 +32,8 @@ 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. + +# Controller parameters kalt = 1 Tialt = 3 Tdalt = 3 @@ -42,7 +49,7 @@ Tdpitch = 1 @mtkmodel Thruster begin @components begin frame_b = Frame() - thrust3d = WorldForce(resolve_frame = :frame_b, scale=0.1, radius=0.02) + thrust3d = WorldForce(resolve_frame = :frame_b, scale=0.1, radius=0.02) # The thrust force is resolved in the local frame of the thruster. thrust = RealInput() end @variables begin @@ -57,12 +64,10 @@ Tdpitch = 1 end end -rou(x) = round(x, digits=3) - -function RotorCraft(; cl = true, addload=true) +function RotorCraft(; closed_loop = true, addload=true) arms = [ BodyCylinder( - r = rou.([arm_length*cos(angle_between_arms*(i-1)), 0, arm_length*sin(angle_between_arms*(i-1))]), + r = [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, @@ -71,32 +76,8 @@ function RotorCraft(; cl = true, addload=true) ] 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] - ) + @named freemotion = FreeMotion(state=true, isroot=true, quat=false) # We use Euler angles to describe the orientation of the rotorcraft. connections = [ connect(world.frame_b, freemotion.frame_a) @@ -106,13 +87,35 @@ function RotorCraft(; cl = true, addload=true) ] systems = [world; arms; body; thrusters; freemotion] if addload + @named load = Body(m = load_mass, air_resistance=0.1) + @named cable = Rope( + l = cable_length, + m = cable_mass, + n = number_of_links, + c = 0, + d = 0, + air_resistance = 0.1, + d_joint = 0.1, + radius = cable_diameter/2, + color = [0.5, 0.4, 0.4, 1], + dir = [0.0, -1, 0] + ) 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 + if closed_loop # add controllers + + # Mixing matrices for the control signals + @parameters Galt[1:4] = ones(4) # The altitude controller affects all thrusters equally + @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) uc = Galt*Calt.ctr_output.u + Groll*Croll.ctr_output.u + Gpitch*Cpitch.ctr_output.u uc = collect(uc) @@ -120,7 +123,7 @@ function RotorCraft(; cl = true, addload=true) append!(connections, [ Calt.err_input.u ~ -body.r_0[2] - Croll.err_input.u ~ freemotion.phi[3] + Croll.err_input.u ~ -freemotion.phi[3] Cpitch.err_input.u ~ -freemotion.phi[1] ]) append!(systems, [Calt; Croll; Cpitch]) @@ -138,12 +141,11 @@ function RotorCraft(; cl = true, addload=true) @named model = ODESystem(connections, t; systems) complete(model) end -model = RotorCraft(cl=true, addload=true) +model = RotorCraft(closed_loop=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 @@ -167,3 +169,4 @@ nothing # hide ![quadrotor animation](quadrotor.gif) +The green arrows in the animation indicate the force applied by the thrusters. diff --git a/ext/Render.jl b/ext/Render.jl index 9d40a696..6e11cb0e 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -253,7 +253,7 @@ function render!(scene, ::typeof(Body), sys, sol, t) end mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) - iszero(r_cm(sol.t[1])) && (return true) + iszero(r_cm(sol.t[1])) && (return false) thing2 = @lift begin # Cylinder Ta = framefun($t) @@ -274,7 +274,7 @@ function render!(scene, ::typeof(Body), sys, sol, t) Makie.GeometryBasics.Cylinder(origin, extremity, cylinder_radius) end mesh!(scene, thing2; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) - true + false end function render!(scene, ::typeof(World), sys, sol, t) From 7c6b3d5af1ca8020e77d9d2596080d59b7f42bb9 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 30 Jul 2024 12:50:28 +0200 Subject: [PATCH 2/2] add world torque component --- src/Multibody.jl | 2 +- src/forces.jl | 47 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/src/Multibody.jl b/src/Multibody.jl index 82007b60..a445fbfe 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -158,7 +158,7 @@ export Revolute, Prismatic, Planar, Spherical, Universal, GearConstraint, Rollin RollingWheel, FreeMotion, RevolutePlanarLoopConstraint include("joints.jl") -export Spring, Damper, SpringDamperParallel, Torque, Force, WorldForce +export Spring, Damper, SpringDamperParallel, Torque, Force, WorldForce, WorldTorque include("forces.jl") export PartialCutForceBaseSensor, BasicCutForce, BasicCutTorque, CutTorque, CutForce, Power diff --git a/src/forces.jl b/src/forces.jl index 31aa56e8..c02d087f 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -75,6 +75,53 @@ Torque acting between two frames, defined by 3 input signals and resolved in fra extend(ODESystem(eqs, t, name = name, systems = [torque, basicTorque]), ptf) end +@component function BasicWorldTorque(; name, resolve_frame = :world) + @named torque = Blocks.RealInput(; nin = 3) + @named frame_b = Frame() + eqs = if resolve_frame == :world + collect(frame_b.tau) .~ -resolve2(ori(frame_b), collect(torque.u)) + elseif resolve_frame == :frame_b + collect(frame_b.tau) .~ -torque.u + else + collect(frame_b.tau) .~ zeros(3) + end |> collect + append!(eqs, collect(frame_b.f) .~ zeros(3)) + ODESystem(eqs, t; name, systems = [torque, frame_b]) +end + +""" + WorldTorque(; name, resolve_frame = :world) + +External torque acting at `frame_b`, defined by 3 input signals and resolved in frame `:world` or `:frame_b`. + +# Connectors: +- `frame_b`: Frame at which the torque is acting +- `torque`: Of type `Blocks.RealInput(3)`. x-, y-, z-coordinates of torque resolved in frame defined by `resolve_frame`. + +# Rendering options +- `scale = 0.1`: scaling factor for the force [m/N] +- `color = [0,1,0,0.5]`: color of the force arrow in rendering +- `radius = 0.05`: radius of the force arrow in rendering +""" +@component function WorldTorque(; name, resolve_frame = :world, scale = 0.1, color = [0, 1, 0, 0.5], radius = 0.05) + @named begin + torque = Blocks.RealInput(; nin = 3) + frame_b = Frame() + basicWorldTorque = BasicWorldTorque(; resolve_frame) + end + pars = @parameters begin + scale = scale, [description = "scaling factor for the force [m/N]"] + color[1:4] = color, [description = "color of the force arrow in rendering"] + radius = radius, [description = "radius of the force arrow in rendering"] + end + eqs = [ + connect(basicWorldTorque.frame_b, frame_b) + connect(basicWorldTorque.torque, torque) + ] + ODESystem(eqs, t, [], pars; name, systems = [torque, basicWorldTorque, frame_b]) +end + + @component function BasicForce(; name, resolve_frame = :frame_b) @named ptf = PartialTwoFrames() @named force = Blocks.RealInput(; nin = 3)