Worldtorque

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,21 +87,43 @@ 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)
  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]
+ 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.

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)

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

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)