add control-design tutorial

docs/src/examples/pendulum.md

@@ -305,3 +305,134 @@ r_A = [r_A; 1] # Homogeneous coordinates
 get_frame(sol, model.lower_arm.frame_a, 12)*r_A
 ```
 the vector is now coinciding with `get_trans(sol, model.lower_arm.frame_b, 12)`.
+
+
+## Pendulum on cart
+
+```@example pendulum
+import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica
+import ModelingToolkitStandardLibrary.Blocks
+using Plots
+W(args...; kwargs...) = Multibody.world
+gray = [0.5, 0.5, 0.5, 1]
+@mtkmodel Cartpole begin
+ @components begin
+ world = W()
+ cart = BodyShape(m = 1, r = [0.2, 0, 0], color=[0.2, 0.2, 0.2, 1], shape="box")
+ mounting_point = FixedTranslation(r = [0.1, 0, 0])
+ prismatic = Prismatic(n = [1, 0, 0], axisflange = true, color=gray, state_priority=100)
+ revolute = Revolute(n = [0, 0, 1], axisflange = false, state_priority=100)
+ pendulum = BodyCylinder(r = [0, 0.5, 0], diameter = 0.015, color=gray)
+ motor = TranslationalModelica.Force(use_support = true)
+ tip = Body(m = 0.05)
+ end
+ @variables begin
+ u(t) = 0
+ x(t)
+ v(t)
+ phi(t)
+ w(t)
+ end
+ @equations begin
+ connect(world.frame_b, prismatic.frame_a)
+ connect(prismatic.frame_b, cart.frame_a, mounting_point.frame_a)
+ connect(mounting_point.frame_b, revolute.frame_a)
+ connect(revolute.frame_b, pendulum.frame_a)
+ connect(pendulum.frame_b, tip.frame_a)
+ connect(motor.flange, prismatic.axis)
+ connect(prismatic.support, motor.support)
+ u ~ motor.f.u
+ x ~ prismatic.s
+ v ~ prismatic.v
+ phi ~ revolute.phi
+ w ~ revolute.w
+ end
+end
+@mtkmodel CartWithInput begin
+ @components begin
+ cart = Cartpole()
+ input = Blocks.Cosine(frequency=1, amplitude=1)
+ end
+ @equations begin
+ connect(input.output, :u, cart.motor.f)
+ end
+end
+@named model = CartWithInput()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+prob = ODEProblem(ssys, [model.cart.prismatic.s => 0.0, model.cart.revolute.phi => 0.1], (0, 10))
+sol = solve(prob, Tsit5())
+plot(sol, layout=4)
+```
+
+```@example pendulum
+import GLMakie
+Multibody.render(model, sol, filename = "cartpole.gif", traces=[model.cart.pendulum.frame_b])
+nothing # hide
+```
+![cartpole](cartpole.gif)
+
+### Adding feedback
+We can add feedback to the cartpole system by connecting the angle of the pendulum to the input of the cart. This is done by adding a [`Blocks.PID`](@ref) controller to the system and connecting the output of the controller to the input of the cart. The controller is then connected to the angle of the pendulum. The controller is set to have a proportional gain of 1, an integral gain of 0.1, and a derivative gain of 0.1. The controller is then connected to the input of the cart.
+
+```@example pendulum
+kx = 30
+kp = 50
+kv = 30
+kw = 10
+
+@mtkmodel CartWithFeedback begin
+ @components begin
+ cart = Cartpole()
+ L = Blocks.MatrixGain(K = -[kx kp kv kw])
+ # reference = Blocks.Step(start_time = 6, height=0.2)
+ end
+ @equations begin
+ L.input.u[1] ~ cart.x
+ L.input.u[2] ~ cart.phi
+ L.input.u[3] ~ cart.v
+ L.input.u[4] ~ cart.w
+ connect(L.output, cart.motor.f)
+ end
+end
+@named model = CartWithFeedback()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+prob = ODEProblem(ssys, [model.cart.prismatic.s => 0.01, model.cart.revolute.phi => 0.01], (0, 5))
+sol = solve(prob, Tsit5())
+plot(sol, idxs=[model.cart.prismatic.s, model.cart.revolute.phi], layout=2)
+```
+
+```@example pendulum
+Multibody.render(model, sol, filename = "inverted_cartpole.gif", x=1, z=1)
+nothing # hide
+```
+![inverted cartpole](inverted_cartpole.gif)
+
+```@example pendulum
+import ModelingToolkit: D_nounits as D
+using LinearAlgebra
+@named cart = Cartpole()
+cart = complete(cart)
+inputs = [cart.u]
+outputs = [cart.x, cart.v, cart.phi, cart.w]
+op = Dict([
+ cart.u => 0
+ cart.tip.r_0[3] => 1
+ vec(ori(cart.tip.frame_a).R .=> I(3))
+ cart.revolute.phi => 0
+ vec(D.(ori(cart.tip.frame_a).R) .=> 0)
+ cart.tip.v_0[3] => 0
+]
+)
+linearize(IRSystem(cart), inputs, outputs; op)
+```
+
+```@example pendulum
+@named cart = Cartpole()
+cart = complete(cart)
+inputs = [cart.u]
+outputs = [cart.x, cart.v, cart.phi, cart.w]
+structural_simplify(IRSystem(cart), ([cart.u], outputs))
+
+```

ext/Render.jl

@@ -79,7 +79,7 @@ function get_color(sys, sol, default, var_name = :color)
  end
 end
 
-function get_shape(sys, sol)::String
+function get_shapefile(sys, sol)::String
  try
  sf = sol(sol.t[1], idxs=collect(sys.shapefile))
  decode(sf)
@@ -88,6 +88,15 @@ function get_shape(sys, sol)::String
  end
 end
 
+function get_shape(sys, sol)::String
+ try
+ sf = sol(sol.t[1], idxs=collect(sys.shape))
+ decode(sf)
+ catch
+ ""
+ end
+end
+
 
 function default_scene(x,y,z; lookat=Vec3f(0,0,0),up=Vec3f(0,1,0),show_axis=false)
  # if string(Makie.current_backend()) == "CairoMakie"
@@ -406,19 +415,53 @@ end
 
 function render!(scene, ::typeof(BodyShape), sys, sol, t)
  color = get_color(sys, sol, :purple)
- shapepath = get_shape(sys, sol)
+ shapepath = get_shapefile(sys, sol)
  if isempty(shapepath)
- radius = Float32(sol(sol.t[1], idxs=sys.radius))
  r_0a = get_fun(sol, collect(sys.frame_a.r_0))
  r_0b = get_fun(sol, collect(sys.frame_b.r_0))
- thing = @lift begin
- r1 = Point3f(r_0a($t))
- r2 = Point3f(r_0b($t))
- origin = r1
- extremity = r2
- Makie.GeometryBasics.Cylinder(origin, extremity, radius)
+ shape = get_shape(sys, sol)
+ if shape == "cylinder"
+ radius = Float32(sol(sol.t[1], idxs=sys.radius))
+ thing = @lift begin
+ r1 = Point3f(r_0a($t))
+ r2 = Point3f(r_0b($t))
+ origin = r1
+ extremity = r2
+ Makie.GeometryBasics.Cylinder(origin, extremity, radius)
+ end
+ mesh!(scene, thing; color, specular = Vec3f(1.5))
+ elseif shape == "box"
+ Rfun = get_rot_fun(sol, sys.frame_a)
+
+ width = Float32(sol(sol.t[1], idxs=sys.radius*2))
+ height = width
+ r = sol(sol.t[1], idxs=collect(sys.r))
+ length = norm(r)
+ length_dir = normalize(r)
+
+ width_dir = [1,0,0]'length_dir > 0.9 ? [0,1,0] : [1,0,0]
+ height_dir = normalize(cross(length_dir, width_dir))
+ width_dir = normalize(cross(height_dir, length_dir))
+
+ r_0a = get_fun(sol, collect(sys.frame_a.r_0)) # Origin is translated by r_shape
+
+ R0 = [length_dir width_dir height_dir]
+ @assert isapprox(det(R0), 1.0, atol=1e-6)
+
+ origin = Vec3f(0, -width/2, -height/2)
+ extent = Vec3f(length, width, height) 
+ thing = Makie.Rect3f(origin, extent)
+ m = mesh!(scene, thing; color, specular = Vec3f(1.5))
+ on(t) do t
+ r1 = Point3f(r_0a(t))
+ R = Rfun(t)
+ q = Rotations.QuatRotation(R*R0).q
+ Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s)
+ Makie.transform!(m, translation=r1, rotation=Q)
+ end
+ else
+ error("Shape $shape not supported")
  end
- mesh!(scene, thing; color, specular = Vec3f(1.5))
  else
  T = get_frame_fun(sol, sys.frame_a)
 

src/components.jl

@@ -373,7 +373,9 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
 
 See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with predefined shapes and automatically computed inertial properties based on geometry and density.
 """
-@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, shapefile="", kwargs...)
+@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, shapefile="", 
+ height = 0.1_norm(r), width = height, shape = "cylinder",
+ kwargs...)
  systems = @named begin
  translation = FixedTranslation(r = r)
  body = Body(; r_cm, r_0, kwargs...)
@@ -394,17 +396,21 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p
  ]
 
  shapecode = encode(shapefile)
+ shape = encode(shape)
  @parameters begin
  r[1:3]=r, [
  description = "Vector from frame_a to frame_b resolved in frame_a",
  ]
  radius = radius, [description = "Radius of the body in animations"]
  color[1:4] = color, [description = "Color of the body in animations"]
  shapefile[1:length(shapecode)] = shapecode
+ width = width, [description = """Width of the body in animations (if shape = "box")"""]
+ height = height, [description = """Height of the body in animations (if shape = "box")"""]
+ shape[1:length(shape)] = shape
  end
 
 
- pars = [r; radius; color; shapefile]
+ pars = [r; radius; color; shapefile; height; width; shape]
 
  r_0, v_0, a_0 = collect.((r_0, v_0, a_0))