From 2c486579a5e244c6d8487984e8894edc14e983b5 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 17 Sep 2023 23:14:25 +0300 Subject: [PATCH 01/61] Add basic file structure for `Mechanical.PlanarMechanics` --- src/Mechanical/Mechanical.jl | 1 + .../PlanarMechanics/PlanarMechanics.jl | 17 +++ src/Mechanical/PlanarMechanics/components.jl | 1 + src/Mechanical/PlanarMechanics/joints.jl | 0 test/Mechanical/planar_mechanics.jl | 9 ++ test/runtests.jl | 113 +++++++++--------- 6 files changed, 86 insertions(+), 55 deletions(-) create mode 100644 src/Mechanical/PlanarMechanics/PlanarMechanics.jl create mode 100644 src/Mechanical/PlanarMechanics/components.jl create mode 100644 src/Mechanical/PlanarMechanics/joints.jl create mode 100644 test/Mechanical/planar_mechanics.jl diff --git a/src/Mechanical/Mechanical.jl b/src/Mechanical/Mechanical.jl index 95c9e0204..1a2306c19 100644 --- a/src/Mechanical/Mechanical.jl +++ b/src/Mechanical/Mechanical.jl @@ -10,5 +10,6 @@ include("Translational/Translational.jl") include("TranslationalPosition/TranslationalPosition.jl") include("TranslationalModelica/TranslationalModelica.jl") include("MultiBody2D/MultiBody2D.jl") +include("PlanarMechanics/PlanarMechanics.jl") end diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl new file mode 100644 index 000000000..345965e6c --- /dev/null +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -0,0 +1,17 @@ +""" +Library to model planar mechanical multi-body systems inspired by https://github.com/dzimmer/PlanarMechanics +""" + +module PlanarMechanics + +using ModelingToolkit, Symbolics, IfElse +using ...Blocks: RealInput, RealOutput +import ...@symcheck + +@parameters t +D = Differential(t) + +export Fixed +include("components.jl") + +end diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl new file mode 100644 index 000000000..fa8575e3c --- /dev/null +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -0,0 +1 @@ +@mtkmodel Fixed begin end diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl new file mode 100644 index 000000000..e69de29bb diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl new file mode 100644 index 000000000..ee9dfc910 --- /dev/null +++ b/test/Mechanical/planar_mechanics.jl @@ -0,0 +1,9 @@ +using ModelingToolkit, OrdinaryDiffEq, Test +using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics + +@parameters t +D = Differential(t) + +@testset "Fixed" begin + @test true +end diff --git a/test/runtests.jl b/test/runtests.jl index 178984894..0470319f2 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,66 +1,69 @@ using SafeTestsets # Blocks -@safetestset "Blocks: math" begin - include("Blocks/math.jl") -end -@safetestset "Blocks: nonlinear" begin - include("Blocks/nonlinear.jl") -end -@safetestset "Blocks: continuous" begin - include("Blocks/continuous.jl") -end -@safetestset "Blocks: sources" begin - include("Blocks/sources.jl") -end -@safetestset "Blocks: analysis points" begin - include("Blocks/test_analysis_points.jl") -end +# @safetestset "Blocks: math" begin +# include("Blocks/math.jl") +# end +# @safetestset "Blocks: nonlinear" begin +# include("Blocks/nonlinear.jl") +# end +# @safetestset "Blocks: continuous" begin +# include("Blocks/continuous.jl") +# end +# @safetestset "Blocks: sources" begin +# include("Blocks/sources.jl") +# end +# @safetestset "Blocks: analysis points" begin +# include("Blocks/test_analysis_points.jl") +# end -# Electrical -@safetestset "Analog Circuits" begin - include("Electrical/analog.jl") -end +# # Electrical +# @safetestset "Analog Circuits" begin +# include("Electrical/analog.jl") +# end -@safetestset "Chua Circuit Demo" begin - include("chua_circuit.jl") -end +# @safetestset "Chua Circuit Demo" begin +# include("chua_circuit.jl") +# end -@safetestset "Digital Circuits" begin - include("Electrical/digital.jl") -end -@safetestset "Chua Circuit Demo" begin - include("chua_circuit.jl") -end +# @safetestset "Digital Circuits" begin +# include("Electrical/digital.jl") +# end +# @safetestset "Chua Circuit Demo" begin +# include("chua_circuit.jl") +# end -# Thermal -@safetestset "Thermal Circuits" begin - include("Thermal/thermal.jl") -end -@safetestset "Thermal Demo" begin - include("Thermal/demo.jl") -end +# # Thermal +# @safetestset "Thermal Circuits" begin +# include("Thermal/thermal.jl") +# end +# @safetestset "Thermal Demo" begin +# include("Thermal/demo.jl") +# end -# Magnetic -@safetestset "Magnetic" begin - include("Magnetic/magnetic.jl") -end +# # Magnetic +# @safetestset "Magnetic" begin +# include("Magnetic/magnetic.jl") +# end -# Mechanical -@safetestset "Mechanical Rotation" begin - include("Mechanical/rotational.jl") -end -@safetestset "Mechanical Translation" begin - include("Mechanical/translational.jl") -end -@safetestset "Mechanical Translation" begin - include("Mechanical/translational_modelica.jl") -end -@safetestset "Multi-Domain" begin - include("multi_domain.jl") +# # Mechanical +# @safetestset "Mechanical Rotation" begin +# include("Mechanical/rotational.jl") +# end +# @safetestset "Mechanical Translation" begin +# include("Mechanical/translational.jl") +# end +# @safetestset "Mechanical Translation" begin +# include("Mechanical/translational_modelica.jl") +# end +# @safetestset "Multi-Domain" begin +# include("multi_domain.jl") +# end +@safetestset "Planar Mechanics" begin + include("Mechanical/planar_mechanics.jl") end -# Hydraulic -@safetestset "Hydraulic IsothermalCompressible" begin - include("Hydraulic/isothermal_compressible.jl") -end +# # Hydraulic +# @safetestset "Hydraulic IsothermalCompressible" begin +# include("Hydraulic/isothermal_compressible.jl") +# end From 36f913bc18d0b9009ed7341685763b04ca458c74 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 18 Sep 2023 01:42:17 +0300 Subject: [PATCH 02/61] Add `Fixed` --- .../PlanarMechanics/PlanarMechanics.jl | 3 ++ src/Mechanical/PlanarMechanics/components.jl | 30 ++++++++++++++++++- src/Mechanical/PlanarMechanics/utils.jl | 21 +++++++++++++ 3 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 src/Mechanical/PlanarMechanics/utils.jl diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 345965e6c..d76174e43 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -11,6 +11,9 @@ import ...@symcheck @parameters t D = Differential(t) +export Frame +include("utils.jl") + export Fixed include("components.jl") diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index fa8575e3c..b16058d54 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -1 +1,29 @@ -@mtkmodel Fixed begin end +""" + Fixed(; name, r = (0.0, 0.0), phi = 0.0) + +Frame fixed in the planar world frame at a given position and orientation + +# Parameters: + + - `r`: [m, m] Fixed absolute x,y-position, resolved in planarWorld frame + - `phi`: [rad] Fixed angle + +# Connectors: + + - `frame: 2-dim. Coordinate system +""" +@mtkmodel Fixed begin + @parameters begin + r, [description = "Fixed absolute x,y-position, resolved in planarWorld frame"] + phi, [description = "Fixed angle"] + end + + @components begin + frame = Frame(; r = (0.0, 0.0), phi = 0.0) + end + + @equations begin + frame.x, frame.y ~ r + frame.phi ~ phi + end +end diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl new file mode 100644 index 000000000..c86e6d022 --- /dev/null +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -0,0 +1,21 @@ +@connector Frame begin + x(t), [description = "x position"] + y(t), [description = "y position"] + phi(t), [description = "rotation angle (counterclockwise)"] + fx(t), [connect = Flow, description = "force in the x direction"] + fy(t), [connect = Flow, description = "force in the y direction"] + j(t), [connect = Flow, description = "torque (clockwise)"] +end +Base.@doc """ + Frame(;name) + +Coordinate system (2-dim.) fixed to the component with one cut-force and cut-torque + +# States: +- `x`: [m] x position +- `y`: [m] y position +- `phi`: [rad] rotation angle (counterclockwise) +- `fx`: [N] force in the x direction +- `fy`: [N] force in the y direction +- `j`: [N.m] torque (clockwise) +""" Frame \ No newline at end of file From a847e5c5ded298ee81e424d7a50aed41c6844bd2 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 18 Sep 2023 22:27:05 +0300 Subject: [PATCH 03/61] Add `Body` --- src/Mechanical/PlanarMechanics/components.jl | 79 ++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index b16058d54..0bbda6a56 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -27,3 +27,82 @@ Frame fixed in the planar world frame at a given position and orientation frame.phi ~ phi end end + +""" + Body(; name, m, j, r, g = nothing) + +Body component with mass and inertia + +# Parameters: + + - `m`: [kg] mass of the body + - `j`: [kg.m²] inertia of the body with respect to the origin of `frame` along the z-axis of `frame` + - `r`: [m, m] (optional) Translational position x,y-position + - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction + +# States: + + - `rx`: [m] x position + - `ry`: [m] y position + - `vx`: [m/s] x velocity + - `vy`: [m/s] y velocity + - `ax`: [m/s²] x acceleration + - `ay`: [m/s²] y acceleration + - `phi`: [rad] rotation angle (counterclockwise) + - `ω`: [rad/s] angular velocity + - `α`: [rad/s²] angular acceleration + +# Connectors: + + - `frame`: 2-dim. Coordinate system +""" +@component function Body(; name, m, j, r = nothing, gy = nothing) + @named frame = Frame() + pars = @parameters begin + m = m + j = j + gy = gy + end + + vars = @variables begin + fx(t) = 0 + fy(t) = 0 + rx(t) = 0 + ry(t) = 0 + vx(t) = 0 + vy(t) = 0 + ax(t) = 0 + ay(t) = 0 + phi(t) = 0 + ω(t) = 0 + α(t) = 0 + end + + eqs = [ + # velocity is the time derivative of position + rx ~ frame.x, + ry ~ frame.y, + vx ~ D(r_x), + vy ~ D(r_y), + phi ~ frame.phi, + ω ~ D(phi), + # acceleration is the time derivative of velocity + ax ~ D(v_x), + ay ~ D(v_y), + α ~ D(ω), + # newton's law + fx ~ frame.fx, + fy ~ frame.fy, + ax ~ fx / m, + ay ~ ifelse(gy !== nothing, fy / m + gy, fy / m), + j * α ~ frame.j, + ] + + if r !== nothing + rx = r[1] + ry = r[2] + end + + return compose(ODESystem(eqs, t, vars, pars; name = name), + frame) +end From aa40ff0034a8c72c66cf7d068a7762b92c42c96b Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Wed, 20 Sep 2023 03:03:47 +0300 Subject: [PATCH 04/61] Add `PartialTwoFrames` --- src/Mechanical/PlanarMechanics/utils.jl | 30 +++++++++++++++++++------ 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl index c86e6d022..c760cea48 100644 --- a/src/Mechanical/PlanarMechanics/utils.jl +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -12,10 +12,26 @@ Base.@doc """ Coordinate system (2-dim.) fixed to the component with one cut-force and cut-torque # States: -- `x`: [m] x position -- `y`: [m] y position -- `phi`: [rad] rotation angle (counterclockwise) -- `fx`: [N] force in the x direction -- `fy`: [N] force in the y direction -- `j`: [N.m] torque (clockwise) -""" Frame \ No newline at end of file + - `x`: [m] x position + - `y`: [m] y position + - `phi`: [rad] rotation angle (counterclockwise) + - `fx`: [N] force in the x direction + - `fy`: [N] force in the y direction + - `j`: [N.m] torque (clockwise) +""" Frame + +@mtkmodel PartialTwoFrames begin + @components begin + frame_a = Frame() + frame_b = Frame() + end +end + +Base.@doc """ + PartialTwoFrames(;name) +Partial model with two frames + +# Connectors: + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +""" PartialTwoFrames \ No newline at end of file From 55f5c91f0d58e90593346d574cb1f0f1f8d38968 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Wed, 20 Sep 2023 03:49:45 +0300 Subject: [PATCH 05/61] Add `FixedTransaltion` --- .../PlanarMechanics/PlanarMechanics.jl | 4 +- src/Mechanical/PlanarMechanics/components.jl | 49 +++++++++++++++++++ 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index d76174e43..7eadd9f5e 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -11,10 +11,10 @@ import ...@symcheck @parameters t D = Differential(t) -export Frame +export Frame, PartialTwoFrames include("utils.jl") -export Fixed +export Fixed, FixedTranslation include("components.jl") end diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 0bbda6a56..406ad60b0 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -106,3 +106,52 @@ Body component with mass and inertia return compose(ODESystem(eqs, t, vars, pars; name = name), frame) end + +""" + FixedTranslation(; name, l, z) +A fixed translation between two components (rigid rod) + +# Parameters: + + - `r`: [m, m] Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0 + - `l`: [m] Length of vector r + - `z`: [m] Position z of cylinder representing the fixed translation + +# Connectors: + + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +""" +@mtkmodel FixedTranslation begin + @extend frame_a, frame_b = partial_frames = PartialTwoFrames() + + @parameters begin + r, + [ + description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0", + ] + l, [description = "Length of vector r"] + z, [description = "Position z of cylinder representing the fixed translation"] + end + + @variables begin + R, [description = "Rotation matrix"] + r0, [description = "Length of the rod resolved w.r.t to inertal frame"] + end + + @equations begin + # resovle the translation w.r.t. inertial frame + R ~ [[cos(frame_a.phi), -sin(frame_a.phi)]; [sin(frame_a.phi), cos(frame_a.phi)]] + r0 ~ R * r + + # rigidly connect positions + frame_a.x + r0[1] ~ frame_b.x + frame_a.y + r0[2] ~ frame_b.y + frame_a.phi ~ frame_b.phi + + # balancing force including lever principle + frame_a.fx + frame_b.fx ~ 0 + frame_a.fy + frame_b.fy ~ 0 + frame_a.j + frame_b.j + r0 * [frame_b.fy, -frame_b.fx] ~ 0 + end +end From 4e8928260c07a8480733588b900b255ef69efa75 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 22 Sep 2023 15:58:09 +0300 Subject: [PATCH 06/61] Add `Revolute` --- .../PlanarMechanics/PlanarMechanics.jl | 7 ++ src/Mechanical/PlanarMechanics/joints.jl | 67 +++++++++++++++++++ src/Mechanical/PlanarMechanics/utils.jl | 2 +- 3 files changed, 75 insertions(+), 1 deletion(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 7eadd9f5e..6c3ba1de9 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -11,6 +11,13 @@ import ...@symcheck @parameters t D = Differential(t) +module Rotational +# TODO: figure out how to use Rotational directly +using ModelingToolkit +export Flange, Support, Fixed +include("../Rotational/utils.jl") +end + export Frame, PartialTwoFrames include("utils.jl") diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index e69de29bb..2e022b906 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -0,0 +1,67 @@ +""" + Revolute(; name, phi = 0.0, tau = 0.0, use_flange = false) +A revolute joint + +# parameters + - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" + - `phi`: [rad] Intial angular position for the flange + - `tau`: [N.m] Inital Cut torque in the flange + +# states + - `phi(t)`: [rad] angular position + - `ω(t)`: [rad/s] angular velocity + - `α(t)`: [rad/s²] angular acceleration + - `j(t)`: [N.m] torque + +# Connectors + - `frame_a` [Frame](@ref) + - `frame_b` [Frame](@ref) + - `fixed` [Fixed](@ref) if `use_flange == false` + - `flange_a` [Flange](@ref) if `use_flange == true` + - `support` [Support](@ref) if `use_flange == true` + +""" +@component function Revolute(; nane, phi = 0.0, tau = 0.0, use_flange = false) + @named partial_frames = PartialTwoFrames() + @unpack frame_a, frame_b = partial_frames + @named fixed = Rotational.Fixed() + + if use_flange + @named flange_a = Rotational.Flange(phi, tau) + @named support = Rotational.Support() + end + + vars = @varialbes begin + phi = phi, [description = "Anugliar position"] + ω = 0.0, [description = "Angular velocity"] + α = 0.0, [description = "Angular acceleration"] + j = tau, [description = "Torque"] + end + + eqs = [ + ω ~ D(phi), + α ~ D(ω), + # rigidly connect position + frame_a.x ~ frame_b.x, + frame_a.y ~ frame_b.y, + frame_a.phi + phi ~ frame_b.phi, + # balance forces + frame_a.fx + frame_b.fx = 0, + frame_a.fy + frame_b.fy = 0, + # balance torques + frame_a.j + frame_b.j = 0, + frame_a.j = j, + ] + + if use_flange + # actutation torque + push!(eqs, connect(fixed.flange, support)) + else + push!(eqs, j ~ ϕ) + end + + pars = [] + + return compose(ODESystem(eqs, t, vars, pars; name = name), + partial_frames, fixed) +end \ No newline at end of file diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl index c760cea48..b78532cb7 100644 --- a/src/Mechanical/PlanarMechanics/utils.jl +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -34,4 +34,4 @@ Partial model with two frames # Connectors: - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque -""" PartialTwoFrames \ No newline at end of file +""" PartialTwoFrames From 80bf790f86b928b659432b82ef8131c4e79bcfbf Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 23 Sep 2023 01:07:38 +0300 Subject: [PATCH 07/61] Instinate Pendulum components without errors --- .../PlanarMechanics/PlanarMechanics.jl | 31 ++++++++- src/Mechanical/PlanarMechanics/components.jl | 67 +++++++++++-------- src/Mechanical/PlanarMechanics/joints.jl | 22 +++--- test/Mechanical/planar_mechanics.jl | 6 +- 4 files changed, 83 insertions(+), 43 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 6c3ba1de9..f4fd1ad2a 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -14,14 +14,41 @@ D = Differential(t) module Rotational # TODO: figure out how to use Rotational directly using ModelingToolkit -export Flange, Support, Fixed +export Flange, Support include("../Rotational/utils.jl") + +""" + Fixed(;name, phi0 = 0.0) + +Flange fixed in housing at a given angle. + +# Connectors: + + - `flange` [Flange](@ref) + +# Parameters: + + - `phi0`: [`rad`] Fixed offset angle of housing +""" +@mtkmodel Fixed begin + @components begin + flange = Flange() + end + @parameters begin + phi0 = 0.0, [description = "Fixed offset angle of flange"] + end + @equations begin + flange.phi ~ phi0 + end +end end export Frame, PartialTwoFrames include("utils.jl") -export Fixed, FixedTranslation +export Fixed, Body, FixedTranslation include("components.jl") +export Revolute +include("joints.jl") end diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 406ad60b0..e0882cf48 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -5,7 +5,8 @@ Frame fixed in the planar world frame at a given position and orientation # Parameters: - - `r`: [m, m] Fixed absolute x,y-position, resolved in planarWorld frame + - `x`: [m] Fixed absolute x-position, resolved in planarWorld frame + - `y`: [m] Fixed absolute y-position, resolved in planarWorld frame - `phi`: [rad] Fixed angle # Connectors: @@ -14,16 +15,18 @@ Frame fixed in the planar world frame at a given position and orientation """ @mtkmodel Fixed begin @parameters begin - r, [description = "Fixed absolute x,y-position, resolved in planarWorld frame"] - phi, [description = "Fixed angle"] + x = 0, [description = "Fixed absolute x-position, resolved in planarWorld frame"] + y = 0, [description = "Fixed absolute y-position, resolved in planarWorld frame"] + phi = 0, [description = "Fixed angle"] end @components begin - frame = Frame(; r = (0.0, 0.0), phi = 0.0) + frame = Frame() end @equations begin - frame.x, frame.y ~ r + frame.x ~ x + frame.y ~ y frame.phi ~ phi end end @@ -38,7 +41,7 @@ Body component with mass and inertia - `m`: [kg] mass of the body - `j`: [kg.m²] inertia of the body with respect to the origin of `frame` along the z-axis of `frame` - `r`: [m, m] (optional) Translational position x,y-position - - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction + - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to 9.81 # States: @@ -56,7 +59,7 @@ Body component with mass and inertia - `frame`: 2-dim. Coordinate system """ -@component function Body(; name, m, j, r = nothing, gy = nothing) +@component function Body(; name, m, j, r = nothing, gy = 9.81) @named frame = Frame() pars = @parameters begin m = m @@ -82,13 +85,13 @@ Body component with mass and inertia # velocity is the time derivative of position rx ~ frame.x, ry ~ frame.y, - vx ~ D(r_x), - vy ~ D(r_y), + vx ~ D(rx), + vy ~ D(ry), phi ~ frame.phi, ω ~ D(phi), # acceleration is the time derivative of velocity - ax ~ D(v_x), - ay ~ D(v_y), + ax ~ D(vx), + ay ~ D(vy), α ~ D(ω), # newton's law fx ~ frame.fx, @@ -108,14 +111,13 @@ Body component with mass and inertia end """ - FixedTranslation(; name, l, z) + FixedTranslation(; name, r::AbstractArray, l) A fixed translation between two components (rigid rod) # Parameters: - - `r`: [m, m] Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0 - - `l`: [m] Length of vector r - - `z`: [m] Position z of cylinder representing the fixed translation + - `rx`: [m] Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0 + - `ry`: [m] Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0 # Connectors: @@ -126,32 +128,39 @@ A fixed translation between two components (rigid rod) @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @parameters begin - r, + rx, [ - description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0", + description = "Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0", + ] + ry, + [ + description = "Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0", ] - l, [description = "Length of vector r"] - z, [description = "Position z of cylinder representing the fixed translation"] end + # begin + # l = norm([rx, ry]) + # end + @variables begin - R, [description = "Rotation matrix"] - r0, [description = "Length of the rod resolved w.r.t to inertal frame"] + r0x(t), [description = "x-length of the rod resolved w.r.t to inertal frame"] + r0y(t), [description = "y-length of the rod resolved w.r.t to inertal frame"] + cos_phi(t), [description = "cos(phi)"] + sin_phi(t), [description = "sin(phi)"] end @equations begin - # resovle the translation w.r.t. inertial frame - R ~ [[cos(frame_a.phi), -sin(frame_a.phi)]; [sin(frame_a.phi), cos(frame_a.phi)]] - r0 ~ R * r - # rigidly connect positions - frame_a.x + r0[1] ~ frame_b.x - frame_a.y + r0[2] ~ frame_b.y + frame_a.x + rx ~ frame_b.x + frame_a.y + ry ~ frame_b.y frame_a.phi ~ frame_b.phi - # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 - frame_a.j + frame_b.j + r0 * [frame_b.fy, -frame_b.fx] ~ 0 + cos_phi ~ cos(frame_a.phi) + sin_phi ~ sin(frame_a.phi) + r0x ~ cos_phi * rx - sin_phi * ry + r0y ~ sin_phi * rx + cos_phi * rx + frame_a.j + frame_b.j + r0x * (frame_b.fy - frame_a.fy) - r0y * (frame_b.fx - frame_a.fx) ~ 0 end end diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 2e022b906..fcf0c4c28 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -21,7 +21,7 @@ A revolute joint - `support` [Support](@ref) if `use_flange == true` """ -@component function Revolute(; nane, phi = 0.0, tau = 0.0, use_flange = false) +@component function Revolute(; name, phi = 0.0, ω = 0.0, tau = 0.0, use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() @@ -31,11 +31,11 @@ A revolute joint @named support = Rotational.Support() end - vars = @varialbes begin - phi = phi, [description = "Anugliar position"] - ω = 0.0, [description = "Angular velocity"] - α = 0.0, [description = "Angular acceleration"] - j = tau, [description = "Torque"] + vars = @variables begin + phi(t) = phi + ω(t) = ω + α(t) = 0.0 + j(t) = tau end eqs = [ @@ -46,18 +46,18 @@ A revolute joint frame_a.y ~ frame_b.y, frame_a.phi + phi ~ frame_b.phi, # balance forces - frame_a.fx + frame_b.fx = 0, - frame_a.fy + frame_b.fy = 0, + frame_a.fx + frame_b.fx ~ 0, + frame_a.fy + frame_b.fy ~ 0, # balance torques - frame_a.j + frame_b.j = 0, - frame_a.j = j, + frame_a.j + frame_b.j ~ 0, + frame_a.j ~ j, ] if use_flange # actutation torque push!(eqs, connect(fixed.flange, support)) else - push!(eqs, j ~ ϕ) + push!(eqs, j ~ phi) end pars = [] diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index ee9dfc910..a98392b41 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -4,6 +4,10 @@ using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics @parameters t D = Differential(t) -@testset "Fixed" begin +@testset "Pendulum" begin + @named body = Body(m = 1, j = 0.1) + @named revolute = Revolute(phi = 0.0, ω = 0.0) + @named rod = FixedTranslation(rx = 1.0, ry = 0.0) + @named ground = Fixed() @test true end From d2a4c34c1dea3a1f5537b1dab6d323007e2923c9 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 23 Sep 2023 03:17:49 +0300 Subject: [PATCH 08/61] Expose `frame_a, frame_b` in Revolute --- src/Mechanical/PlanarMechanics/joints.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index fcf0c4c28..7d9618faa 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -63,5 +63,5 @@ A revolute joint pars = [] return compose(ODESystem(eqs, t, vars, pars; name = name), - partial_frames, fixed) -end \ No newline at end of file + frame_a, frame_b, fixed) +end From edfe1296d1ea42e71f9a236318de00d52211e6b1 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 23 Sep 2023 03:18:20 +0300 Subject: [PATCH 09/61] Add first test for `PlanarMechanics`: a simple pendulum --- test/Mechanical/planar_mechanics.jl | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index a98392b41..21b16b5a0 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -5,9 +5,27 @@ using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics D = Differential(t) @testset "Pendulum" begin + @named ceiling = Fixed() + @named rod = FixedTranslation(rx = 1.0, ry = 0.0) @named body = Body(m = 1, j = 0.1) @named revolute = Revolute(phi = 0.0, ω = 0.0) - @named rod = FixedTranslation(rx = 1.0, ry = 0.0) - @named ground = Fixed() - @test true + + connections = [ + connect(ceiling.frame, revolute.frame_a), + connect(revolute.frame_b, rod.frame_a), + connect(rod.frame_b, body.frame), + ] + + @named model = ODESystem(connections, + t, + [], + [], + systems = [body, revolute, rod, ceiling]) + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0.0, 60), []; jac = true) + sol = solve(prob, Rodas5P()) + + # phi and omega for the pendulum body + @test length(states(sys)) == 2 end From 6e2244f77da2f3b18d8cf69e6af5a1157c99377b Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 23 Sep 2023 03:19:31 +0300 Subject: [PATCH 10/61] Re-enable all tests --- test/runtests.jl | 112 +++++++++++++++++++++++------------------------ 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/test/runtests.jl b/test/runtests.jl index 0470319f2..e6ff5be77 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,69 +1,69 @@ using SafeTestsets # Blocks -# @safetestset "Blocks: math" begin -# include("Blocks/math.jl") -# end -# @safetestset "Blocks: nonlinear" begin -# include("Blocks/nonlinear.jl") -# end -# @safetestset "Blocks: continuous" begin -# include("Blocks/continuous.jl") -# end -# @safetestset "Blocks: sources" begin -# include("Blocks/sources.jl") -# end -# @safetestset "Blocks: analysis points" begin -# include("Blocks/test_analysis_points.jl") -# end +@safetestset "Blocks: math" begin + include("Blocks/math.jl") +end +@safetestset "Blocks: nonlinear" begin + include("Blocks/nonlinear.jl") +end +@safetestset "Blocks: continuous" begin + include("Blocks/continuous.jl") +end +@safetestset "Blocks: sources" begin + include("Blocks/sources.jl") +end +@safetestset "Blocks: analysis points" begin + include("Blocks/test_analysis_points.jl") +end -# # Electrical -# @safetestset "Analog Circuits" begin -# include("Electrical/analog.jl") -# end +# Electrical +@safetestset "Analog Circuits" begin + include("Electrical/analog.jl") +end -# @safetestset "Chua Circuit Demo" begin -# include("chua_circuit.jl") -# end +@safetestset "Chua Circuit Demo" begin + include("chua_circuit.jl") +end -# @safetestset "Digital Circuits" begin -# include("Electrical/digital.jl") -# end -# @safetestset "Chua Circuit Demo" begin -# include("chua_circuit.jl") -# end +@safetestset "Digital Circuits" begin + include("Electrical/digital.jl") +end +@safetestset "Chua Circuit Demo" begin + include("chua_circuit.jl") +end -# # Thermal -# @safetestset "Thermal Circuits" begin -# include("Thermal/thermal.jl") -# end -# @safetestset "Thermal Demo" begin -# include("Thermal/demo.jl") -# end +# Thermal +@safetestset "Thermal Circuits" begin + include("Thermal/thermal.jl") +end +@safetestset "Thermal Demo" begin + include("Thermal/demo.jl") +end -# # Magnetic -# @safetestset "Magnetic" begin -# include("Magnetic/magnetic.jl") -# end +# Magnetic +@safetestset "Magnetic" begin + include("Magnetic/magnetic.jl") +end -# # Mechanical -# @safetestset "Mechanical Rotation" begin -# include("Mechanical/rotational.jl") -# end -# @safetestset "Mechanical Translation" begin -# include("Mechanical/translational.jl") -# end -# @safetestset "Mechanical Translation" begin -# include("Mechanical/translational_modelica.jl") -# end -# @safetestset "Multi-Domain" begin -# include("multi_domain.jl") -# end +# Mechanical +@safetestset "Mechanical Rotation" begin + include("Mechanical/rotational.jl") +end +@safetestset "Mechanical Translation" begin + include("Mechanical/translational.jl") +end +@safetestset "Mechanical Translation" begin + include("Mechanical/translational_modelica.jl") +end +@safetestset "Multi-Domain" begin + include("multi_domain.jl") +end @safetestset "Planar Mechanics" begin include("Mechanical/planar_mechanics.jl") end -# # Hydraulic -# @safetestset "Hydraulic IsothermalCompressible" begin -# include("Hydraulic/isothermal_compressible.jl") -# end +# Hydraulic +@safetestset "Hydraulic IsothermalCompressible" begin + include("Hydraulic/isothermal_compressible.jl") +end From a4fba77a11542211197914f58a9c21081d172809 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 23 Sep 2023 08:22:53 +0300 Subject: [PATCH 11/61] import rotational components properly --- .../PlanarMechanics/PlanarMechanics.jl | 33 +------------------ 1 file changed, 1 insertion(+), 32 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index f4fd1ad2a..f596f537c 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -7,42 +7,11 @@ module PlanarMechanics using ModelingToolkit, Symbolics, IfElse using ...Blocks: RealInput, RealOutput import ...@symcheck +import ModelingToolkitStandardLibrary.Mechanical.Rotational @parameters t D = Differential(t) -module Rotational -# TODO: figure out how to use Rotational directly -using ModelingToolkit -export Flange, Support -include("../Rotational/utils.jl") - -""" - Fixed(;name, phi0 = 0.0) - -Flange fixed in housing at a given angle. - -# Connectors: - - - `flange` [Flange](@ref) - -# Parameters: - - - `phi0`: [`rad`] Fixed offset angle of housing -""" -@mtkmodel Fixed begin - @components begin - flange = Flange() - end - @parameters begin - phi0 = 0.0, [description = "Fixed offset angle of flange"] - end - @equations begin - flange.phi ~ phi0 - end -end -end - export Frame, PartialTwoFrames include("utils.jl") From b7ef27652179836f51eb90a8a57256fbcbd2fcbe Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 24 Sep 2023 03:36:03 +0300 Subject: [PATCH 12/61] Add `Prismatic` --- .../PlanarMechanics/PlanarMechanics.jl | 5 +- src/Mechanical/PlanarMechanics/components.jl | 4 - src/Mechanical/PlanarMechanics/joints.jl | 102 +++++++++++++++++- .../TranslationalModelica.jl | 2 +- test/Mechanical/planar_mechanics.jl | 8 ++ 5 files changed, 110 insertions(+), 11 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index f596f537c..f3ce5595c 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -4,10 +4,11 @@ Library to model planar mechanical multi-body systems inspired by https://github module PlanarMechanics +import ModelingToolkitStandardLibrary.Mechanical.Rotational +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica using ModelingToolkit, Symbolics, IfElse using ...Blocks: RealInput, RealOutput import ...@symcheck -import ModelingToolkitStandardLibrary.Mechanical.Rotational @parameters t D = Differential(t) @@ -18,6 +19,6 @@ include("utils.jl") export Fixed, Body, FixedTranslation include("components.jl") -export Revolute +export Revolute, Prismatic include("joints.jl") end diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index e0882cf48..aa9be288d 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -138,10 +138,6 @@ A fixed translation between two components (rigid rod) ] end - # begin - # l = norm([rx, ry]) - # end - @variables begin r0x(t), [description = "x-length of the rod resolved w.r.t to inertal frame"] r0y(t), [description = "y-length of the rod resolved w.r.t to inertal frame"] diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 7d9618faa..9a8eb3bb2 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -25,10 +25,13 @@ A revolute joint @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() + systems = [frame_a, frame_b, fixed] if use_flange - @named flange_a = Rotational.Flange(phi, tau) + @named flange_a = Rotational.Flange(; phi, tau) + push!(systems, flange_a) @named support = Rotational.Support() + push!(systems, support) end vars = @variables begin @@ -41,7 +44,7 @@ A revolute joint eqs = [ ω ~ D(phi), α ~ D(ω), - # rigidly connect position + # rigidly connect positions frame_a.x ~ frame_b.x, frame_a.y ~ frame_b.y, frame_a.phi + phi ~ frame_b.phi, @@ -54,14 +57,105 @@ A revolute joint ] if use_flange + push!(eqs, connect(fixed.flange, support)) + else # actutation torque + push!(eqs, j ~ 0) + end + + pars = [] + + return compose(ODESystem(eqs, t, vars, pars; name = name), + systems...) +end + +""" + Prismatic(; name, rx, ry, f, s = 0, use_flange = false) +A prismatic joint + +# parameters + - `rx`: [m] x-direction of the rod wrt. body system at phi=0 + - `ry`: [m] y-direction of the rod wrt. body system at phi=0 + - `ex`: [m] x-component of unit vector in direction of r + - `ey`: [m] y-component of unit vector in direction of r + - `f`: [N] Force in direction of elongation + - `s`: [m] Elongation of the joint" + - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" + +# states + - `s(t)`: [m] Elongation of the joint + - `v(t)`: [m/s] Velocity of elongation + - `a(t)`: [m/s²] Acceleration of elongation + - `e0x(t)`: [m] x-component of unit vector resolved w.r.t inertial frame + - `e0y(t)`: [m] y-component of unit vector resolved w.r.t inertial frame + - `r0x(t)`: [m] x-component of the rod resolved w.r.t to inertal frame + - `r0y(t)`: [m] y-length of the rod resolved w.r.t to inertal frame + - `cos_phi(t)`: [degree] cos(phi) + - `sin_phi(t)`: [degree] sin(phi) + + +# Connectors + - `frame_a` [Frame](@ref) + - `frame_b` [Frame](@ref) + - `fixed` [Fixed](@ref) if `use_flange == false` + - `flange_a` [Flange](@ref) if `use_flange == true` + - `support` [Support](@ref) if `use_flange == true` +""" +@component function Prismatic(; name, rx, ry, ex, ey, f = 0, s = 0, use_flange = false) + @named partial_frames = PartialTwoFrames() + @unpack frame_a, frame_b = partial_frames + @named fixed = TranslationalModelica.Support() + systems = [frame_a, frame_b, fixed] + + if use_flange + @named flange_a = TranslationalModelica.Flange(; f, s) + push!(systems, flange_a) + @named support = TranslationalModelica.Support() + push!(systems, support) + end + + vars = @variables begin + s(t) = 0.0 + v(t) = 0.0 + a(t) = 0.0 + e0x(t) + e0y(t) + r0x(t) + r0y(t) + cos_phi(t) + sin_phi(t) + end + + eqs = [ + v ~ D(s), + a ~ D(v), + # rigidly connect positions + frame_a.x + rx ~ frame_b.x, + frame_a.y + ry ~ frame_b.y, + frame_a.phi ~ frame_b.phi, + # balance forces + frame_a.fx + frame_b.fx ~ 0, + frame_a.fy + frame_b.fy ~ 0, + # balance torques + cos_phi ~ cos(frame_a.phi), + sin_phi ~ sin(frame_a.phi), + e0x ~ cos_phi * ex - sin_phi * ey, + e0y ~ sin_phi * ex + cos_phi * ey, + r0x ~ e0x * s, + r0y ~ e0y * s, + frame_a.j + frame_b.j + r0x * (frame_b.fy - frame_a.fy) - r0y * (frame_b.fx - frame_a.fx) ~ 0, + frame_a.fx * e0y - frame_a.fy * e0x ~ f, + ] + + if use_flange push!(eqs, connect(fixed.flange, support)) else - push!(eqs, j ~ phi) + # actutation torque + push!(eqs, f ~ 0) end pars = [] return compose(ODESystem(eqs, t, vars, pars; name = name), - frame_a, frame_b, fixed) + systems...) end diff --git a/src/Mechanical/TranslationalModelica/TranslationalModelica.jl b/src/Mechanical/TranslationalModelica/TranslationalModelica.jl index 2aec491b1..7eb5a61e8 100644 --- a/src/Mechanical/TranslationalModelica/TranslationalModelica.jl +++ b/src/Mechanical/TranslationalModelica/TranslationalModelica.jl @@ -9,7 +9,7 @@ using ...Blocks: RealInput, RealOutput @parameters t D = Differential(t) -export Flange +export Flange, Support include("utils.jl") export Fixed, Mass, Spring, Damper diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 21b16b5a0..a1a8cd217 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -29,3 +29,11 @@ D = Differential(t) # phi and omega for the pendulum body @test length(states(sys)) == 2 end + +@testset "Prismatic" begin + r = [1.0, 0.0] + e = r / sqrt(r' * r) + @named prismatic = Prismatic(rx = r[1], ry = r[2], ex = e[1], ey = e[2]) + # just testing instantiation + @test true +end \ No newline at end of file From 21b97b70dc9ac9bfd2b8e085a6f8687361707fc4 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 24 Sep 2023 03:41:16 +0300 Subject: [PATCH 13/61] Newline --- test/Mechanical/planar_mechanics.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index a1a8cd217..ce3a34dce 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -36,4 +36,4 @@ end @named prismatic = Prismatic(rx = r[1], ry = r[2], ex = e[1], ey = e[2]) # just testing instantiation @test true -end \ No newline at end of file +end From 5e914ab983a90b219b1ec28a80ab79242d72b3f9 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 24 Sep 2023 09:43:08 +0300 Subject: [PATCH 14/61] Fix gravity direction --- src/Mechanical/PlanarMechanics/components.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index aa9be288d..d5bdf364e 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -41,7 +41,7 @@ Body component with mass and inertia - `m`: [kg] mass of the body - `j`: [kg.m²] inertia of the body with respect to the origin of `frame` along the z-axis of `frame` - `r`: [m, m] (optional) Translational position x,y-position - - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to 9.81 + - `gy`: [m/s²] (optional) gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to -9.807 # States: @@ -59,7 +59,7 @@ Body component with mass and inertia - `frame`: 2-dim. Coordinate system """ -@component function Body(; name, m, j, r = nothing, gy = 9.81) +@component function Body(; name, m, j, r = nothing, gy = -9.807) @named frame = Frame() pars = @parameters begin m = m From c8cc29d1b397c73e230a1516973e844c4da35dd6 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 24 Sep 2023 09:43:24 +0300 Subject: [PATCH 15/61] Add free body test case --- test/Mechanical/planar_mechanics.jl | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index ce3a34dce..c713ef7a9 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -1,8 +1,30 @@ using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics +# using Plots @parameters t D = Differential(t) +tspan = (0.0, 3.0) + +@testset "Free body" begin + @named body = Body(m = 1, j = 0.1) + @named model = ODESystem(Equation[], + t, + [], + [], + systems = [body]) + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) + + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + + free_falling_displacement = -0.5 * 9.807 * tspan[2]^2 # 0.5 * g * t^2 + @test sol[body.ry][end] ≈ free_falling_displacement + @test sol[body.rx][end] == 0 # no horizontal displacement + # plot(sol, idxs = [body.rx, body.ry]) +end @testset "Pendulum" begin @named ceiling = Fixed() @@ -23,7 +45,7 @@ D = Differential(t) systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0.0, 60), []; jac = true) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) # phi and omega for the pendulum body From cc6c47b799d2febd47ab7d9ff062614b06c2f91c Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 28 Sep 2023 02:15:48 +0300 Subject: [PATCH 16/61] Add `ResolveInFrameA` type --- src/Mechanical/MultiBody2D/types.jl | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/Mechanical/MultiBody2D/types.jl diff --git a/src/Mechanical/MultiBody2D/types.jl b/src/Mechanical/MultiBody2D/types.jl new file mode 100644 index 000000000..3f000904b --- /dev/null +++ b/src/Mechanical/MultiBody2D/types.jl @@ -0,0 +1,14 @@ + +@enum ResolveInFrameA begin + world + frame_a + frame_resolve +end + +Base.@doc """ + Enumeration to define the frame in which an absolute vector is resolved (world, frame_a, frame_resolve) + Values: + - `world`: Resolve in world frame + - `frame_a`: Resolve in frame_a" + - `frame_resolve`: Resolve in frame_resolve (frame_resolve must be connected) +""" ResolveInFrameA \ No newline at end of file From 335510f12a4e595c38e8d0cc8eaa6a3c2e6ff6d5 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 28 Sep 2023 11:13:15 +0300 Subject: [PATCH 17/61] Alias `FrameResolve` to `Frame` --- src/Mechanical/PlanarMechanics/utils.jl | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl index b78532cb7..a5b5cd0a0 100644 --- a/src/Mechanical/PlanarMechanics/utils.jl +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -20,6 +20,10 @@ Coordinate system (2-dim.) fixed to the component with one cut-force and cut-tor - `j`: [N.m] torque (clockwise) """ Frame +# extends Frame with just styling +# https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Interfaces/Frame_resolve.mo +FrameResolve = Frame + @mtkmodel PartialTwoFrames begin @components begin frame_a = Frame() From e0b077baa863051fd9d1576c08d53d00456fd560 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 28 Sep 2023 11:33:16 +0300 Subject: [PATCH 18/61] Add `ZeroPosition` interface --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/utils.jl | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index f3ce5595c..3e0dcdeea 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -13,7 +13,7 @@ import ...@symcheck @parameters t D = Differential(t) -export Frame, PartialTwoFrames +export Frame, FrameResolve, PartialTwoFrames, ZeroPosition include("utils.jl") export Fixed, Body, FixedTranslation diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl index a5b5cd0a0..69710331d 100644 --- a/src/Mechanical/PlanarMechanics/utils.jl +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -39,3 +39,22 @@ Partial model with two frames - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque """ PartialTwoFrames + +""" + ZeroPosition(;name) +Set zero position vector and orientation object of frame_resolve + +# Connectors: + - `frame_resolve` [FrameResolve](@ref) Coordinate system fixed to the component with one cut-force and cut-torque +""" +@mtkmodel ZeroPosition begin + @components begin + frame_resolve = FrameResolve() + end + + @equations begin + frame_resolve.x ~ 0 + frame_resolve.y ~ 0 + frame_resolve.phi ~ 0 + end +end \ No newline at end of file From 10c7e9fcad21851b326f311a92cb0218da833a70 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 29 Sep 2023 03:38:51 +0300 Subject: [PATCH 19/61] Add `AbsolutePosition` sensor --- src/Mechanical/MultiBody2D/types.jl | 14 -- .../PlanarMechanics/PlanarMechanics.jl | 3 + src/Mechanical/PlanarMechanics/sensors.jl | 141 ++++++++++++++++++ test/Mechanical/planar_mechanics.jl | 6 + 4 files changed, 150 insertions(+), 14 deletions(-) delete mode 100644 src/Mechanical/MultiBody2D/types.jl create mode 100644 src/Mechanical/PlanarMechanics/sensors.jl diff --git a/src/Mechanical/MultiBody2D/types.jl b/src/Mechanical/MultiBody2D/types.jl deleted file mode 100644 index 3f000904b..000000000 --- a/src/Mechanical/MultiBody2D/types.jl +++ /dev/null @@ -1,14 +0,0 @@ - -@enum ResolveInFrameA begin - world - frame_a - frame_resolve -end - -Base.@doc """ - Enumeration to define the frame in which an absolute vector is resolved (world, frame_a, frame_resolve) - Values: - - `world`: Resolve in world frame - - `frame_a`: Resolve in frame_a" - - `frame_resolve`: Resolve in frame_resolve (frame_resolve must be connected) -""" ResolveInFrameA \ No newline at end of file diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 3e0dcdeea..3fe56a555 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -21,4 +21,7 @@ include("components.jl") export Revolute, Prismatic include("joints.jl") + +export AbsolutePosition +include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl new file mode 100644 index 000000000..fbbb60362 --- /dev/null +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -0,0 +1,141 @@ +""" + PartialAbsoluteSensor(;name) +Partial absolute sensor model for sensors defined by components +# Connectors: + + - `frame: 2-dim. Coordinate system +""" +@mtkmodel PartialAbsoluteSensor begin + @components begin + frame_a = Frame() + end +end + +""" + PartialAbsoluteBaseSensor(;name) +Partial absolute sensor models for sensors defined by equations (frame_resolve must be connected exactly once) +# Connectors: + + - `frame_a`: 2-dim. Coordinate system from which kinematic quantities are measured + - `frame_resolve`: 2-dim. Coordinate system in which vector is optionally resolved +""" +@mtkmodel PartialAbsoluteBaseSensor begin + @components begin + frame_a = Frame() + frame_resolve = FrameResolve() + end + + @equations begin + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/PartialAbsoluteBaseSensor.mo#L20-L21 + frame_a.fx ~ 0 + frame_a.fy ~ 0 + frame_a.j ~ 0 + frame_resolve.fx ~ 0 + frame_resolve.fy ~ 0 + frame_resolve.j ~ 0 + end +end + +""" + BasicAbsolutePosition(;name, resolve_in_frame = :world) +Measure absolute position and orientation (same as Sensors.AbsolutePosition, but frame_resolve is not conditional and must be connected). + +# Connectors: + + - `x`: [m] x-position + - `y`: [m] y-position + - `phi`: [rad] rotation angle (counterclockwise) + - `frame_a`: 2-dim. Coordinate system from which kinematic quantities are measured + - `frame_resolve`: 2-dim. Coordinate system in which vector is optionally resolved + +# Parameters: + + - `resolve_in_frame`: Frame in which output vector r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) +""" +@component function BasicAbsolutePosition(; name, resolve_in_frame = :world) + @named x = RealOutput() + @named y = RealOutput() + @named phi = RealOutput() + + @named partial_abs_base_sensor = PartialAbsoluteBaseSensor() + @unpack frame_a, frame_resolve = partial_abs_base_sensor + + if resolve_in_frame == :world + r = [ + frame_a.x, + frame_a.y, + frame_a.phi] + elseif resolve_in_frame == :frame_a + rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0; + sin(frame_a.phi) cos(frame_a.phi) 0; + 0 0 1] + r = transpose(rotation_matrix) * [frame_a.x, frame_a.y, frame_a.phi] - + [0, 0, frame_a.phi] + elseif resolve_in_frame == :frame_resolve + rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0; + sin(frame_resolve.phi) cos(frame_resolve.phi) 0; + 0 0 1] + r = transpose(rotation_matrix) * [frame_a.x, frame_a.y, frame_a.phi] - + [0, 0, frame_resolve.phi] + else + error("frame_resolve_type must be one of :world, :frame_a, :frame_resolve") + end + + eqs = [ + x ~ r[1], + y ~ r[2], + phi ~ r[3], + ] + + return compose(ODESystem(eqs, t, [], []; name = name), + x, y, phi, frame_a, frame_resolve) +end + +""" + AbsolutePosition(;name, resolve_in_frame = :world) +Measure absolute position and orientation of the origin of frame connector + +# Connectors: + + - `x`: [m] x-position + - `y`: [m] y-position + - `phi`: [rad] rotation angle (counterclockwise) + +# Parameters: + + - `resolve_in_frame`: Frame in which output vector r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) +""" +@component function AbsolutePosition(; name, resolve_in_frame = :world) + @named pos = BasicAbsolutePosition() + @named partial_abs_sensor = PartialAbsoluteSensor() + @unpack frame_a, = partial_abs_sensor + + @named x = RealOutput() + @named y = RealOutput() + @named phi = RealOutput() + + systems = [pos, frame_a, x, y, phi] + + eqs = [ + pos.x ~ x, + pos.y ~ y, + pos.phi ~ phi, + connect(pos.frame_a, frame_a), + ] + + if resolve_in_frame == :frame_resolve + @named fr = FrameResolve() + push!(systems, fr) + push!(eqs, connect(pos.frame_resolve, fr)) + end + + if resolve_in_frame != :frame_resolve + @named zero_position = ZeroPosition() + push!(systems, zero_position) + push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), + systems...) +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index c713ef7a9..4cd683e17 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -59,3 +59,9 @@ end # just testing instantiation @test true end + +@testset "Measure Demo" begin + @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :world) + @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :frame_a) + @test_nowarn @named abs_pos_fr = AbsolutePosition(; resolve_in_frame = :frame_resolve) +end From 20eba4835af74f880cf8812e052513aca6b770aa Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 29 Sep 2023 14:25:38 +0300 Subject: [PATCH 20/61] Add `PartialRelativeSensor` --- src/Mechanical/PlanarMechanics/sensors.jl | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index fbbb60362..2d3afac50 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -11,6 +11,24 @@ Partial absolute sensor model for sensors defined by components end end +""" + PartialRelativeSensor(;name) +Partial relative sensor model for sensors defined by components + +# Connectors: + + - `frame_a`: Coordinate system a + - `frame_b`: Coordinate system b +""" +@mtkmodel PartialRelativeSensor begin + @components begin + frame_a = Frame() + frame_b = Frame() + end + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/PartialRelativeSensor.mo#L12-L13 +end + """ PartialAbsoluteBaseSensor(;name) Partial absolute sensor models for sensors defined by equations (frame_resolve must be connected exactly once) From c467b05b4cef48940e6be0b65316b3250e3f2633 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 29 Sep 2023 14:26:21 +0300 Subject: [PATCH 21/61] Fix `reslove_in_frame` default value --- src/Mechanical/PlanarMechanics/sensors.jl | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 2d3afac50..39a55296a 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -56,7 +56,7 @@ Partial absolute sensor models for sensors defined by equations (frame_resolve m end """ - BasicAbsolutePosition(;name, resolve_in_frame = :world) + BasicAbsolutePosition(;name, resolve_in_frame = :frame_a) Measure absolute position and orientation (same as Sensors.AbsolutePosition, but frame_resolve is not conditional and must be connected). # Connectors: @@ -64,14 +64,14 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but - `x`: [m] x-position - `y`: [m] y-position - `phi`: [rad] rotation angle (counterclockwise) - - `frame_a`: 2-dim. Coordinate system from which kinematic quantities are measured + - `frame_a`: Coordinate system a - `frame_resolve`: 2-dim. Coordinate system in which vector is optionally resolved # Parameters: - - `resolve_in_frame`: Frame in which output vector r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) + - `resolve_in_frame`: Frame in which output x, y, phi r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) """ -@component function BasicAbsolutePosition(; name, resolve_in_frame = :world) +@component function BasicAbsolutePosition(; name, resolve_in_frame = :frame_a) @named x = RealOutput() @named y = RealOutput() @named phi = RealOutput() @@ -97,7 +97,7 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but r = transpose(rotation_matrix) * [frame_a.x, frame_a.y, frame_a.phi] - [0, 0, frame_resolve.phi] else - error("frame_resolve_type must be one of :world, :frame_a, :frame_resolve") + error("resolve_in_frame must be one of :world, :frame_a, :frame_resolve") end eqs = [ @@ -111,7 +111,7 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but end """ - AbsolutePosition(;name, resolve_in_frame = :world) + AbsolutePosition(;name, resolve_in_frame = :frame_a) Measure absolute position and orientation of the origin of frame connector # Connectors: @@ -122,9 +122,9 @@ Measure absolute position and orientation of the origin of frame connector # Parameters: - - `resolve_in_frame`: Frame in which output vector r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) + - `resolve_in_frame`: Frame in which output x, y, phi is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) """ -@component function AbsolutePosition(; name, resolve_in_frame = :world) +@component function AbsolutePosition(; name, resolve_in_frame = :frame_a) @named pos = BasicAbsolutePosition() @named partial_abs_sensor = PartialAbsoluteSensor() @unpack frame_a, = partial_abs_sensor From 603d8731306b1d32686e9b9a1431bf6c53a99739 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 29 Sep 2023 15:15:37 +0300 Subject: [PATCH 22/61] Add `RelativePostion` component --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/sensors.jl | 140 ++++++++++++++++++ test/Mechanical/planar_mechanics.jl | 7 +- 3 files changed, 147 insertions(+), 2 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 3fe56a555..ded850429 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -22,6 +22,6 @@ include("components.jl") export Revolute, Prismatic include("joints.jl") -export AbsolutePosition +export AbsolutePosition, RelativePosition include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 39a55296a..896810ea5 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -55,6 +55,38 @@ Partial absolute sensor models for sensors defined by equations (frame_resolve m end end +""" + PartialRelativeBaseSensor(;name) +Partial relative sensor models for sensors defined by equations (frame_resolve must be connected exactly once) + +# Connectors: + - `frame_a`: + - `frame_b`: + - `frame_resolve`: +""" +@mtkmodel PartialRelativeBaseSensor begin + @components begin + frame_a = Frame() + frame_b = Frame() + frame_resolve = FrameResolve() + end + + @equations begin + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/PartialRelativeBaseSensor.mo#L19-L21 + + frame_a.fx ~ 0 + frame_a.fy ~ 0 + frame_a.j ~ 0 + frame_b.fx ~ 0 + frame_b.fy ~ 0 + frame_b.j ~ 0 + frame_resolve.fx ~ 0 + frame_resolve.fy ~ 0 + frame_resolve.j ~ 0 + end +end + """ BasicAbsolutePosition(;name, resolve_in_frame = :frame_a) Measure absolute position and orientation (same as Sensors.AbsolutePosition, but frame_resolve is not conditional and must be connected). @@ -157,3 +189,111 @@ Measure absolute position and orientation of the origin of frame connector return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +""" + BasicRelativePosition(; name, resolve_in_frame = :frame_a) +Measure relative position and orientation between the origins of two frame connectors + +# Connectors: + + - `x`: [m] x-position + - `y`: [m] y-position + - `phi`: [rad] rotation angle (counterclockwise) + - `frame_a`: Coordinate system a + - `frame_b`: Coordinate system b + - `frame_resolve`: + +# Parameters: + + - `resolve_in_frame`: Frame in which output x, y, phi is resolved (1: :world, 2: :frame_a, 3: frame_b 4: :frame_resolve) +""" +@component function BasicRelativePosition(; name, resolve_in_frame = :frame_a) + @named x = RealOutput() + @named y = RealOutput() + @named phi = RealOutput() + + @named partial_rel_pos = PartialRelativeBaseSensor() + @unpack frame_a, frame_b, frame_resolve = partial_rel_pos + + if resolve_in_frame == :frame_a + rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0; + sin(frame_a.phi) cos(frame_a.phi) 0; + 0 0 1] + r = transpose(rotation_matrix) * + [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] + elseif resolve_in_frame == :frame_b + rotation_matrix = [cos(frame_b.phi) -sin(frame_b.phi) 0; + sin(frame_b.phi) cos(frame_b.phi) 0; + 0 0 1] + r = transpose(rotation_matrix) * + [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] + elseif resolve_in_frame == :world + r = [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] + elseif resolve_in_frame == :frame_resolve + rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0; + sin(frame_resolve.phi) cos(frame_resolve.phi) 0; + 0 0 1] + r = transpose(rotation_matrix) * + [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] + else + error("resolve_in_frame must be one of :world, :frame_a, :frame_resolve") + end + + eqs = [ + x ~ r[1], + y ~ r[2], + phi ~ r[3], + ] + + return compose(ODESystem(eqs, t, [], []; name = name), + x, y, phi, frame_a, frame_b, frame_resolve) +end + +""" + RelativePosition(; name, resolve_in_frame = :frame_a) +Measure relative position and orientation between the origins of two frame connectors + +# Connectors: + + - `x`: [m] x-position + - `y`: [m] y-position + - `phi`: [rad] rotation angle (counterclockwise) + - `frame_a`: Coordinate system a + - `frame_b`: Coordinate system b +# Parameters: + + - `resolve_in_frame`: Frame in which output x, y, phi is resolved (1: :world, 2: :frame_a, 3: frame_b 4: :frame_resolve) +""" +@component function RelativePosition(; name, resolve_in_frame = :frame_a) + @named pos = BasicRelativePosition(; resolve_in_frame) + @named partial_rel_pos = PartialRelativeSensor() + @unpack frame_a, frame_b = partial_rel_pos + + @named x = RealOutput() + @named y = RealOutput() + @named phi = RealOutput() + + systems = [pos, frame_a, frame_b, x, y, phi] + eqs = [ + pos.x ~ x, + pos.y ~ y, + pos.phi ~ phi, + connect(pos.frame_a, frame_a), + connect(pos.frame_b, frame_b), + ] + + if resolve_in_frame == :frame_resolve + @named fr = FrameResolve() + push!(systems, fr) + push!(eqs, connect(pos.frame_resolve, fr)) + end + + if resolve_in_frame != :frame_resolve + @named zero_position = ZeroPosition() + push!(systems, zero_position) + push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), + systems...) +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 4cd683e17..739adf87b 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -61,7 +61,12 @@ end end @testset "Measure Demo" begin - @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :world) + @test_nowarn @named abs_pos_w = AbsolutePosition(; resolve_in_frame = :world) @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :frame_a) @test_nowarn @named abs_pos_fr = AbsolutePosition(; resolve_in_frame = :frame_resolve) + + @test_nowarn @named rel_pos_w = RelativePosition(; resolve_in_frame = :world) + @test_nowarn @named rel_pos_fa = RelativePosition(; resolve_in_frame = :frame_a) + @test_nowarn @named rel_pos_fb = RelativePosition(; resolve_in_frame = :frame_b) + @test_nowarn @named rel_pos_fr = RelativePosition(; resolve_in_frame = :frame_resolve) end From 813e5bc2381a843e756c19698b115ef4383479b2 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 29 Sep 2023 17:16:24 +0300 Subject: [PATCH 23/61] Fix `RealOut` connections --- src/Mechanical/PlanarMechanics/sensors.jl | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 896810ea5..f004fbf85 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -9,6 +9,8 @@ Partial absolute sensor model for sensors defined by components @components begin frame_a = Frame() end + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/PartialAbsoluteSensor.mo#L11 end """ @@ -133,9 +135,9 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but end eqs = [ - x ~ r[1], - y ~ r[2], - phi ~ r[3], + x.u ~ r[1], + y.u ~ r[2], + phi.u ~ r[3], ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -168,9 +170,9 @@ Measure absolute position and orientation of the origin of frame connector systems = [pos, frame_a, x, y, phi] eqs = [ - pos.x ~ x, - pos.y ~ y, - pos.phi ~ phi, + x.u ~ pos.x, + y.u ~ pos.y, + phi.u ~ pos.phi, connect(pos.frame_a, frame_a), ] From 8b7a26195552b86e2f002fdacee79d2e5ef9a135 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sat, 30 Sep 2023 08:20:16 +0300 Subject: [PATCH 24/61] Add a test for `AbsolutePosition` sensor --- src/Mechanical/PlanarMechanics/sensors.jl | 9 ++++---- test/Mechanical/planar_mechanics.jl | 28 +++++++++++++++++++++++ 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index f004fbf85..c4eb60170 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -170,9 +170,9 @@ Measure absolute position and orientation of the origin of frame connector systems = [pos, frame_a, x, y, phi] eqs = [ - x.u ~ pos.x, - y.u ~ pos.y, - phi.u ~ pos.phi, + x.u ~ pos.x.u, + y.u ~ pos.y.u, + phi.u ~ pos.phi.u, connect(pos.frame_a, frame_a), ] @@ -188,8 +188,7 @@ Measure absolute position and orientation of the origin of frame connector push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), - systems...) + return compose(ODESystem(eqs, t, [], []; name = name), systems...) end """ diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 739adf87b..358f662c3 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -60,6 +60,34 @@ end @test true end +@testset "Position Sensors" begin + m = 1 + @named body = Body(; m, j = 0.1) + @named abs_pos_sensor = AbsolutePosition() + connections = [ + connect(body.frame, abs_pos_sensor.frame_a), + body.phi ~ 0, + body.fx ~ 0, + body.fy ~ m * -9.807, + ] + + @named model = ODESystem(connections, + t, + [], + [], + systems = [body, abs_pos_sensor]) + + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) + + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + + @test sol[abs_pos_sensor.frame_a.y][end] ≈ sol[body.ry][end] + @test sol[abs_pos_sensor.frame_a.x][end] ≈ sol[body.rx][end] +end + @testset "Measure Demo" begin @test_nowarn @named abs_pos_w = AbsolutePosition(; resolve_in_frame = :world) @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :frame_a) From e8f9809556dc91b12d1c2938da382906f224d4c1 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 1 Oct 2023 11:57:44 +0300 Subject: [PATCH 25/61] Add `Position Sensors (two free falling bodies)` test --- src/Mechanical/PlanarMechanics/sensors.jl | 12 +++--- src/Mechanical/PlanarMechanics/utils.jl | 2 +- test/Mechanical/planar_mechanics.jl | 46 ++++++++++++++++++----- 3 files changed, 43 insertions(+), 17 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index c4eb60170..0d3d0cfda 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -241,9 +241,9 @@ Measure relative position and orientation between the origins of two frame conne end eqs = [ - x ~ r[1], - y ~ r[2], - phi ~ r[3], + x.u ~ r[1], + y.u ~ r[2], + phi.u ~ r[3], ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -276,9 +276,9 @@ Measure relative position and orientation between the origins of two frame conne systems = [pos, frame_a, frame_b, x, y, phi] eqs = [ - pos.x ~ x, - pos.y ~ y, - pos.phi ~ phi, + pos.x.u ~ x.u, + pos.y.u ~ y.u, + pos.phi.u ~ phi.u, connect(pos.frame_a, frame_a), connect(pos.frame_b, frame_b), ] diff --git a/src/Mechanical/PlanarMechanics/utils.jl b/src/Mechanical/PlanarMechanics/utils.jl index 69710331d..f243a6116 100644 --- a/src/Mechanical/PlanarMechanics/utils.jl +++ b/src/Mechanical/PlanarMechanics/utils.jl @@ -57,4 +57,4 @@ Set zero position vector and orientation object of frame_resolve frame_resolve.y ~ 0 frame_resolve.phi ~ 0 end -end \ No newline at end of file +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 358f662c3..8b1f34fe3 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -60,22 +60,38 @@ end @test true end -@testset "Position Sensors" begin +@testset "Position Sensors (two free falling bodies)" begin m = 1 - @named body = Body(; m, j = 0.1) - @named abs_pos_sensor = AbsolutePosition() + j = 0 + resolve_in_frame = :world + + @named body1 = Body(; m, j) + @named body2 = Body(; m, j) + @named base = Fixed() + + @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) + @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) + @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) + connections = [ - connect(body.frame, abs_pos_sensor.frame_a), - body.phi ~ 0, - body.fx ~ 0, - body.fy ~ m * -9.807, + connect(body1.frame, abs_pos_sensor.frame_a), + connect(rel_pos_sensor1.frame_a, body1.frame), + connect(rel_pos_sensor1.frame_b, base.frame), + connect(rel_pos_sensor2.frame_b, body1.frame), + connect(rel_pos_sensor2.frame_a, body2.frame), + body1.phi ~ 0, + body1.fx ~ 0, + body1.fy ~ m * -9.807, + body2.phi ~ 0, + body2.fx ~ 0, + body2.fy ~ m * -9.807, ] @named model = ODESystem(connections, t, [], [], - systems = [body, abs_pos_sensor]) + systems = [body1, body2, base, abs_pos_sensor, rel_pos_sensor1, rel_pos_sensor2]) sys = structural_simplify(model) unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) @@ -84,8 +100,18 @@ end sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) - @test sol[abs_pos_sensor.frame_a.y][end] ≈ sol[body.ry][end] - @test sol[abs_pos_sensor.frame_a.x][end] ≈ sol[body.rx][end] + # the two bodyies falled the same distance, and so the absolute sensor attached to body1 + @test sol[abs_pos_sensor.y.u][end] ≈ sol[body1.ry][end] ≈ sol[body2.ry][end] + + # sensor1 is attached to body1, so the relative y-position between body1 and the base is + # equal to the y-position of body1 + @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.y.u][end] + + # the relative y-position between body1 and body2 is zero + @test sol[rel_pos_sensor2.y.u][end] == 0 + + # no displacement in the x-direction + @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] end @testset "Measure Demo" begin From 9f3809de41806c07b7366c1078472a0904bc2685 Mon Sep 17 00:00:00 2001 From: Arno Strouwen Date: Tue, 26 Sep 2023 00:52:41 +0200 Subject: [PATCH 26/61] Documenter 1.0 upgrade --- .github/workflows/CI.yml | 4 ++++ .github/workflows/FormatCheck.yml | 2 +- docs/Project.toml | 3 +-- docs/make.jl | 10 ++------- docs/src/index.md | 35 ++++++++++--------------------- 5 files changed, 19 insertions(+), 35 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 023acbf5e..48a0f9232 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -3,9 +3,13 @@ on: pull_request: branches: - main + paths-ignore: + - 'docs/**' push: branches: - main + paths-ignore: + - 'docs/**' jobs: test: runs-on: ubuntu-latest diff --git a/.github/workflows/FormatCheck.yml b/.github/workflows/FormatCheck.yml index dd551501c..45bd09c47 100644 --- a/.github/workflows/FormatCheck.yml +++ b/.github/workflows/FormatCheck.yml @@ -3,7 +3,7 @@ name: format-check on: push: branches: - - 'master' + - 'main' - 'release-' tags: '*' pull_request: diff --git a/docs/Project.toml b/docs/Project.toml index e38c82671..034ba981d 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -11,9 +11,8 @@ Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" [compat] ControlSystemsBase = "1.1" DifferentialEquations = "7.6" -Documenter = "0.27" +Documenter = "1" IfElse = "0.1" ModelingToolkit = "8.67" -ModelingToolkitStandardLibrary = "2" OrdinaryDiffEq = "6.31" Plots = "1.36" diff --git a/docs/make.jl b/docs/make.jl index ed1072c80..3a6e05ec8 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -19,14 +19,6 @@ include("pages.jl") makedocs(sitename = "ModelingToolkitStandardLibrary.jl", authors = "Julia Computing", - clean = true, - doctest = false, - linkcheck = true, - strict = [ - :linkcheck, - :doctest, - :example_block, - ], modules = [ModelingToolkit, ModelingToolkitStandardLibrary, ModelingToolkitStandardLibrary.Blocks, @@ -38,6 +30,8 @@ makedocs(sitename = "ModelingToolkitStandardLibrary.jl", ModelingToolkitStandardLibrary.Thermal, ModelingToolkitStandardLibrary.Hydraulic, ModelingToolkitStandardLibrary.Hydraulic.IsothermalCompressible], + clean = true, doctest = false, linkcheck = true, + warnonly = [:docs_block, :missing_docs, :cross_references], format = Documenter.HTML(assets = ["assets/favicon.ico"], canonical = "https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/"), pages = pages) diff --git a/docs/src/index.md b/docs/src/index.md index 90aed9b00..e6e05c0c4 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -87,32 +87,19 @@ Pkg.status(; mode = PKGMODE_MANIFEST) # hide ``` -```@raw html -You can also download the -manifest file and the -project file. +link_manifest = "https://github.com/SciML/" * name * ".jl/tree/gh-pages/v" * version * + "/assets/Manifest.toml" +link_project = "https://github.com/SciML/" * name * ".jl/tree/gh-pages/v" * version * + "/assets/Project.toml" +Markdown.parse("""You can also download the +[manifest]($link_manifest) +file and the +[project]($link_project) +file. +""") ``` From 7c6710ca078f945e33fe8da183be54f5d1533c3d Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 25 Sep 2023 18:14:41 +0200 Subject: [PATCH 27/61] avoid using array variable --- src/Blocks/math.jl | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Blocks/math.jl b/src/Blocks/math.jl index 9dc91a600..9fb8ea3af 100644 --- a/src/Blocks/math.jl +++ b/src/Blocks/math.jl @@ -24,11 +24,11 @@ end Gain.f(k; name) = Gain.f(; k, name) """ - MatrixGain(K::AbstractArray; name) + MatrixGain(; K::AbstractArray, name) Output the product of a gain matrix with the input signal vector. -# Parameters: +# Structural parameters: - `K`: Matrix gain @@ -38,19 +38,19 @@ Output the product of a gain matrix with the input signal vector. - `output` """ @mtkmodel MatrixGain begin - @parameters begin - K, [description = "Matrix gain"] + @structural_parameters begin + K end begin - nout = size(getdefault(K), 1) - nin = size(getdefault(K), 2) + nout = size(K, 1) + nin = size(K, 2) end @components begin input = RealInput(; nin = nin) output = RealOutput(; nout = nout) end @equations begin - [output.u[i] ~ sum(getdefault(K)[i, j] * input.u[j] for j in 1:nin) + [output.u[i] ~ sum(K[i, j] * input.u[j] for j in 1:nin) for i in 1:nout]... end end From 3ada901022ac2d0252bf13bfb32cfbd4dd464b06 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 26 Sep 2023 08:17:19 +0200 Subject: [PATCH 28/61] Update Project.toml --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index e4f737e98..c779dad39 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelingToolkitStandardLibrary" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" authors = ["Chris Rackauckas and Julia Computing"] -version = "2.3.1" +version = "2.3.2" [deps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" From d5a29d71f1b358bd9a3f79eaeae13edc876446c0 Mon Sep 17 00:00:00 2001 From: Yingbo Ma Date: Tue, 26 Sep 2023 10:04:59 -0400 Subject: [PATCH 29/61] Fix heterogeneous parameters handling for `linearize_function` Co-authored-by: Fredrik Bagge Carlson --- src/Blocks/analysis_points.jl | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/Blocks/analysis_points.jl b/src/Blocks/analysis_points.jl index a6b90866d..9b4fec8b2 100644 --- a/src/Blocks/analysis_points.jl +++ b/src/Blocks/analysis_points.jl @@ -450,10 +450,17 @@ end # Methods above are implemented in terms of linearization_function, the method below creates wrappers for linearize for f in [:get_sensitivity, :get_comp_sensitivity, :get_looptransfer] - @eval function $f(sys, ap, args...; loop_openings = nothing, kwargs...) - lin_fun, ssys = $(Symbol(string(f) * "_function"))(sys, ap, args...; loop_openings, + @eval function $f(sys, + ap, + args...; + loop_openings = nothing, + op = Dict(), + p = DiffEqBase.NullParameters(), + kwargs...) + lin_fun, ssys = $(Symbol(string(f) * "_function"))(sys, ap, args...; op, p, + loop_openings, kwargs...) - ModelingToolkit.linearize(ssys, lin_fun; kwargs...), ssys + ModelingToolkit.linearize(ssys, lin_fun; op, p, kwargs...), ssys end end From 2e93f4ab6475973b03561b010401b3771622b4dd Mon Sep 17 00:00:00 2001 From: Yingbo Ma Date: Tue, 26 Sep 2023 11:28:21 -0400 Subject: [PATCH 30/61] Relax tests --- test/Thermal/thermal.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/Thermal/thermal.jl b/test/Thermal/thermal.jl index d8f69fd41..bf8b39383 100644 --- a/test/Thermal/thermal.jl +++ b/test/Thermal/thermal.jl @@ -240,7 +240,7 @@ end @test sol.retcode == Success @test sol[T_winding.T] == sol[winding.T] @test sol[T_core.T] == sol[core.T] - @test sol[-core.port.Q_flow] == + @test sol[-core.port.Q_flow] ≈ sol[coreLosses.port.Q_flow + convection.solid.Q_flow + winding2core.port_b.Q_flow] @test sol[T_winding.T][end] >= 500 # not good but better than nothing @test sol[T_core.T] <= sol[T_winding.T] From 2cd8394a6602bfce4a981e211fb4ec2e497fe416 Mon Sep 17 00:00:00 2001 From: Yingbo Ma Date: Tue, 26 Sep 2023 13:20:29 -0400 Subject: [PATCH 31/61] Update Project.toml --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index c779dad39..3dcd582b4 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelingToolkitStandardLibrary" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" authors = ["Chris Rackauckas and Julia Computing"] -version = "2.3.2" +version = "2.3.3" [deps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" From e47232b8503441a3beef68dd462c4d3f8b794332 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 29 Sep 2023 08:27:28 +0200 Subject: [PATCH 32/61] add info annotation with common mistake --- src/Blocks/analysis_points.jl | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/Blocks/analysis_points.jl b/src/Blocks/analysis_points.jl index 9b4fec8b2..e38402b38 100644 --- a/src/Blocks/analysis_points.jl +++ b/src/Blocks/analysis_points.jl @@ -520,6 +520,11 @@ get_comp_sensitivity Compute the (linearized) loop-transfer function in analysis point `ap`, from `ap.out` to `ap.in`. +!!! info "Negative feedback" + + Feedback loops often use negative feedback, and the computed loop-transfer function will in this case have the negative feedback included. Standard analysis tools often assume a loop-transfer function without the negative gain built in, and the result of this function may thus need negation before use. + + !!! danger "Experimental" The analysis-point interface is currently experimental and at any time subject to breaking changes not respecting semantic versioning. From 7d58e06400066969e348a1669c4e72e444d19426 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 1 Oct 2023 14:05:42 +0300 Subject: [PATCH 33/61] Rename `{x,y,phi}` -> `rel_{x,y,phi} in relative sensors --- src/Mechanical/PlanarMechanics/sensors.jl | 40 +++++++++++------------ test/Mechanical/planar_mechanics.jl | 4 +-- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 0d3d0cfda..10867c64b 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -197,9 +197,9 @@ Measure relative position and orientation between the origins of two frame conne # Connectors: - - `x`: [m] x-position - - `y`: [m] y-position - - `phi`: [rad] rotation angle (counterclockwise) + - `rel_x`: [m] Relative x-position + - `rel_y`: [m] Relative y-position + - `rel_phi`: [rad] Relative rotation angle (counterclockwise) - `frame_a`: Coordinate system a - `frame_b`: Coordinate system b - `frame_resolve`: @@ -209,9 +209,9 @@ Measure relative position and orientation between the origins of two frame conne - `resolve_in_frame`: Frame in which output x, y, phi is resolved (1: :world, 2: :frame_a, 3: frame_b 4: :frame_resolve) """ @component function BasicRelativePosition(; name, resolve_in_frame = :frame_a) - @named x = RealOutput() - @named y = RealOutput() - @named phi = RealOutput() + @named rel_x = RealOutput() + @named rel_y = RealOutput() + @named rel_phi = RealOutput() @named partial_rel_pos = PartialRelativeBaseSensor() @unpack frame_a, frame_b, frame_resolve = partial_rel_pos @@ -241,13 +241,13 @@ Measure relative position and orientation between the origins of two frame conne end eqs = [ - x.u ~ r[1], - y.u ~ r[2], - phi.u ~ r[3], + rel_x.u ~ r[1], + rel_y.u ~ r[2], + rel_phi.u ~ r[3], ] return compose(ODESystem(eqs, t, [], []; name = name), - x, y, phi, frame_a, frame_b, frame_resolve) + rel_x, rel_y, rel_phi, frame_a, frame_b, frame_resolve) end """ @@ -256,9 +256,9 @@ Measure relative position and orientation between the origins of two frame conne # Connectors: - - `x`: [m] x-position - - `y`: [m] y-position - - `phi`: [rad] rotation angle (counterclockwise) + - `rel_x`: [m] Relative x-position + - `re_y`: [m] Relative y-position + - `rel_phi`: [rad] Relative rotation angle (counterclockwise) - `frame_a`: Coordinate system a - `frame_b`: Coordinate system b # Parameters: @@ -270,15 +270,15 @@ Measure relative position and orientation between the origins of two frame conne @named partial_rel_pos = PartialRelativeSensor() @unpack frame_a, frame_b = partial_rel_pos - @named x = RealOutput() - @named y = RealOutput() - @named phi = RealOutput() + @named rel_x = RealOutput() + @named rel_y = RealOutput() + @named rel_phi = RealOutput() - systems = [pos, frame_a, frame_b, x, y, phi] + systems = [pos, frame_a, frame_b, rel_x, rel_y, rel_phi] eqs = [ - pos.x.u ~ x.u, - pos.y.u ~ y.u, - pos.phi.u ~ phi.u, + pos.rel_x.u ~ rel_x.u, + pos.rel_y.u ~ rel_y.u, + pos.rel_phi.u ~ rel_phi.u, connect(pos.frame_a, frame_a), connect(pos.frame_b, frame_b), ] diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 8b1f34fe3..ac19ee35d 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -105,10 +105,10 @@ end # sensor1 is attached to body1, so the relative y-position between body1 and the base is # equal to the y-position of body1 - @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.y.u][end] + @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.rel_y.u][end] # the relative y-position between body1 and body2 is zero - @test sol[rel_pos_sensor2.y.u][end] == 0 + @test sol[rel_pos_sensor2.rel_y.u][end] == 0 # no displacement in the x-direction @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] From 82dcbccc28dc8e01738a2f45e42d67bbe49da37c Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 1 Oct 2023 15:52:31 +0300 Subject: [PATCH 34/61] Add `BasicTransformAbsoluteVector` --- src/Mechanical/PlanarMechanics/sensors.jl | 91 +++++++++++++++++++++++ 1 file changed, 91 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 10867c64b..a7b09889b 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -298,3 +298,94 @@ Measure relative position and orientation between the origins of two frame conne return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function BasicTransformAbsoluteVector(; + name, + frame_in = :frame_a, + frame_out = frame_in) + @named frame_a = Frame() + @named frame_b = Frame() + + @named x_in = RealInput() + @named y_in = RealInput() + @named phi_in = RealInput() + + @named x_out = RealOutput() + @named y_out = RealOutput() + @named phi_out = RealOutput() + + @named frame_resolve = FrameResolve() + + systems = [frame_a, frame_b, frame_resolve, x_in, y_in, phi_in, x_out, y_out, phi_out] + eqs = [ + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/BasicTransformAbsoluteVector.mo#L42-L43 + frame_a.fx ~ 0, + frame_a.fy ~ 0, + frame_a.j ~ 0, + frame_resolve.fx ~ 0, + frame_resolve.fy ~ 0, + frame_resolve.j ~ 0, + ] + + r_temp = Vector{Float64}(undef, 3) + R1 = Matrix{Float64}(undef, 3, 4) + + if frame_out == frame_in + append!(eqs, [ + x_out.u ~ x_in.u, + y_out.u ~ y_in.u, + phi_out.u ~ phi_in.u, + ]) + else + if frame_in == :world + R1 = [1.0 0.0 0.0 0.0; + 0.0 1.0 0.0 0.0; + 0.0 0.0 1.0 0.0] + elseif frame_in == :frame_a + R1 = [cos(frame_a.phi) -sin(frame_a.phi) 0.0 0.0; + sin(frame_a.phi) cos(frame_a.phi) 0.0 0.0; + 0.0 0.0 1.0 frame_a.phi] + elseif frame_in == :frame_resolve + R1 = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0 0.0; + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0 0.0; + 0.0 0.0 1.0 frame_resolve.phi] + else + error("Wrong value for parameter frame_in") + end + + r_temp = R1 * [x_in.u, y_in.u, phi_in.u, 1] + + if frame_out == :world + append!(eqs, [ + x_out.u ~ r_temp[1], + y_out.u ~ r_temp[2], + phi_out.u ~ r_temp[3], + ]) + elseif frame_out == :frame_a + rotation_matrix = [cos(frame_a.phi) sin(frame_a.phi) 0.0; + -sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] + r = rotation_matrix * r_temp + append!(eqs, [ + x_out.u ~ r[1], + y_out.u ~ r[2], + phi_out.u ~ r[3], + ]) + elseif frame_out == :frame_resolve + rotation_matrix = [cos(frame_resolve.phi) sin(frame_resolve.phi) 0.0; + -sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] + r = rotation_matrix * r_temp + append!(eqs, [ + x_out.u ~ r[1], + y_out.u ~ r[2], + phi_out.u ~ r[3], + ]) + else + error("Wrong value for parameter frame_out") + end + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end From 57992e9f6b95a31e8ece66a0cc8c626294637e02 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 1 Oct 2023 17:55:50 +0300 Subject: [PATCH 35/61] Add `TransformAbsoluteVector` --- src/Mechanical/PlanarMechanics/sensors.jl | 45 +++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index a7b09889b..cfbdd2005 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -389,3 +389,48 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function TransformAbsoluteVector(; + name, + frame_in = :frame_a, + frame_out = frame_in) + @named frame_a = Frame() + + @named x_in = RealInput() + @named y_in = RealInput() + @named phi_in = RealInput() + + @named x_out = RealOutput() + @named y_out = RealOutput() + @named phi_out = RealOutput() + @named basic_transformb_vector = BasicTransformAbsoluteVector(; frame_in, frame_out) + + systems = [frame_a, x_in, y_in, phi_in, x_out, y_out, phi_out, basic_transformb_vector] + + eqs = [ + connect(basic_transformb_vector.frame_a, frame_a), + # out + connect(basic_transformb_vector.x_out, x_out), + connect(basic_transformb_vector.y_out, y_out), + connect(basic_transformb_vector.phi_out, phi_out), + # in + connect(basic_transformb_vector.x_in, x_in), + connect(basic_transformb_vector.y_in, y_in), + connect(basic_transformb_vector.phi_in, phi_in), + ] + + if frame_in == :frame_resolve || frame_out == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(basic_transformb_vector.frame_resolve, frame_resolve)) + end + + if !(frame_in == :frame_resolve || frame_out == :frame_resolve) + @named zero_position = ZeroPosition() + push!(systems, zero_position) + push!(eqs, + connect(zero_position.frame_resolve, basic_transformb_vector.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end From 86e359bc0af62b7eae279fc4e0624d9183e1834f Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 1 Oct 2023 19:56:33 +0300 Subject: [PATCH 36/61] Add `AbsoluteVelocity` sensor --- src/Mechanical/PlanarMechanics/sensors.jl | 55 +++++++++++++++++++++-- 1 file changed, 52 insertions(+), 3 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index cfbdd2005..10c5bee6b 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -426,10 +426,59 @@ end end if !(frame_in == :frame_resolve || frame_out == :frame_resolve) - @named zero_position = ZeroPosition() - push!(systems, zero_position) + @named zero_pos = ZeroPosition() + push!(systems, zero_pos) + push!(eqs, + connect(zero_pos.frame_resolve, basic_transformb_vector.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end + +@component function AbsoluteVelocity(; name, resolve_in_frame = :frame_a) + @named partial_abs_sensor = PartialAbsoluteSensor() + @unpack frame_a, = partial_abs_sensor + + @named v_x = RealOutput() + @named v_y = RealOutput() + @named ω = RealOutput() + + @named pos = BasicAbsolutePosition(; resolve_in_frame = :world) + @named zero_pos = ZeroPosition() + + @named transform_absolute_vector = TransformAbsoluteVector(; + frame_in = :world, + frame_out = resolve_in_frame) + + systems = [frame_a, v_x, v_y, ω, pos, zero_pos, transform_absolute_vector] + + eqs = [ + # connect(position.r, der1.u), + # connect(der1.y, transformAbsoluteVector.r_in) + D(pos.x.u) ~ transform_absolute_vector.x_in.u, + D(pos.y.u) ~ transform_absolute_vector.y_in.u, + D(pos.phi.u) ~ transform_absolute_vector.phi_in.u, + # connect(transformAbsoluteVector.r_out, v) + transform_absolute_vector.x_out.u ~ v_x.u, + transform_absolute_vector.y_out.u ~ v_y.u, + transform_absolute_vector.phi_out.u ~ ω.u, + connect(pos.frame_a, frame_a), + connect(zero_pos.frame_resolve, pos.frame_resolve), + connect(transform_absolute_vector.frame_a, frame_a), + ] + + if resolve_in_frame == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(transform_absolute_vector.frame_resolve, frame_resolve)) + end + + if resolve_in_frame != :frame_resolve + @named zero_pos1 = ZeroPosition() + push!(systems, zero_pos1) push!(eqs, - connect(zero_position.frame_resolve, basic_transformb_vector.frame_resolve)) + connect(transform_absolute_vector.zero_pos.frame_resolve, + zero_pos1.frame_resolve)) end return compose(ODESystem(eqs, t, [], []; name = name), systems...) From 87ee5bae6b144c92f3a6a583aaa0959d322aaf52 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 10:24:46 +0300 Subject: [PATCH 37/61] Test `AbsoluteVelocity` --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- test/Mechanical/planar_mechanics.jl | 46 +++++++++++-------- 2 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index ded850429..4c0778f77 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -22,6 +22,6 @@ include("components.jl") export Revolute, Prismatic include("joints.jl") -export AbsolutePosition, RelativePosition +export AbsolutePosition, RelativePosition, AbsoluteVelocity include("sensors.jl") end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index ac19ee35d..c1269931a 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -5,9 +5,12 @@ using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics @parameters t D = Differential(t) tspan = (0.0, 3.0) +g = -9.807 @testset "Free body" begin - @named body = Body(m = 1, j = 0.1) + m = 2 + j = 1 + @named body = Body(; m, j) @named model = ODESystem(Equation[], t, [], @@ -20,9 +23,10 @@ tspan = (0.0, 3.0) sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) - free_falling_displacement = -0.5 * 9.807 * tspan[2]^2 # 0.5 * g * t^2 + free_falling_displacement = 0.5 * g * tspan[end]^2 # 0.5 * g * t^2 @test sol[body.ry][end] ≈ free_falling_displacement @test sol[body.rx][end] == 0 # no horizontal displacement + @test all(sol[body.phi] .== 0) # plot(sol, idxs = [body.rx, body.ry]) end @@ -62,7 +66,7 @@ end @testset "Position Sensors (two free falling bodies)" begin m = 1 - j = 0 + j = 1 resolve_in_frame = :world @named body1 = Body(; m, j) @@ -70,6 +74,7 @@ end @named base = Fixed() @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) + @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) @@ -79,19 +84,23 @@ end connect(rel_pos_sensor1.frame_b, base.frame), connect(rel_pos_sensor2.frame_b, body1.frame), connect(rel_pos_sensor2.frame_a, body2.frame), - body1.phi ~ 0, - body1.fx ~ 0, - body1.fy ~ m * -9.807, - body2.phi ~ 0, - body2.fx ~ 0, - body2.fy ~ m * -9.807, + connect(body1.frame, abs_v_sensor.frame_a), + [s ~ 0 for s in (body1.phi, body2.phi, body1.fx, body1.fy, body2.fx, body2.fy)]..., ] @named model = ODESystem(connections, t, [], [], - systems = [body1, body2, base, abs_pos_sensor, rel_pos_sensor1, rel_pos_sensor2]) + systems = [ + body1, + body2, + base, + abs_pos_sensor, + abs_v_sensor, + rel_pos_sensor1, + rel_pos_sensor2, + ]) sys = structural_simplify(model) unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) @@ -101,7 +110,8 @@ end @test SciMLBase.successful_retcode(sol) # the two bodyies falled the same distance, and so the absolute sensor attached to body1 - @test sol[abs_pos_sensor.y.u][end] ≈ sol[body1.ry][end] ≈ sol[body2.ry][end] + @test sol[abs_pos_sensor.y.u][end] ≈ sol[body1.ry][end] ≈ sol[body2.ry][end] ≈ + 0.5 * g * tspan[end]^2 # sensor1 is attached to body1, so the relative y-position between body1 and the base is # equal to the y-position of body1 @@ -112,15 +122,13 @@ end # no displacement in the x-direction @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] + + # velocity after t seconds v = g * t + @test sol[abs_v_sensor.v_y.u][end] ≈ g * tspan[end] end @testset "Measure Demo" begin - @test_nowarn @named abs_pos_w = AbsolutePosition(; resolve_in_frame = :world) - @test_nowarn @named abs_pos_fa = AbsolutePosition(; resolve_in_frame = :frame_a) - @test_nowarn @named abs_pos_fr = AbsolutePosition(; resolve_in_frame = :frame_resolve) - - @test_nowarn @named rel_pos_w = RelativePosition(; resolve_in_frame = :world) - @test_nowarn @named rel_pos_fa = RelativePosition(; resolve_in_frame = :frame_a) - @test_nowarn @named rel_pos_fb = RelativePosition(; resolve_in_frame = :frame_b) - @test_nowarn @named rel_pos_fr = RelativePosition(; resolve_in_frame = :frame_resolve) + @test_nowarn @named abs_v_w = AbsoluteVelocity(; resolve_in_frame = :world) + @test_nowarn @named abs_v_fa = AbsoluteVelocity(; resolve_in_frame = :frame_a) + @test_nowarn @named abs_v_fr = AbsoluteVelocity(; resolve_in_frame = :frame_resolve) end From 6bd87a59c45cf2bacbac139b24185d06b8751723 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 14:12:10 +0300 Subject: [PATCH 38/61] Add `BasicTransformRelativeVector` --- src/Mechanical/PlanarMechanics/sensors.jl | 90 ++++++++++++++++++++++- 1 file changed, 87 insertions(+), 3 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 10c5bee6b..7dc203e21 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -303,9 +303,6 @@ end name, frame_in = :frame_a, frame_out = frame_in) - @named frame_a = Frame() - @named frame_b = Frame() - @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -314,6 +311,8 @@ end @named y_out = RealOutput() @named phi_out = RealOutput() + @named frame_a = Frame() + @named frame_b = Frame() @named frame_resolve = FrameResolve() systems = [frame_a, frame_b, frame_resolve, x_in, y_in, phi_in, x_out, y_out, phi_out] @@ -483,3 +482,88 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@named function BasicTransformRelativeVector(; + name, + frame_in = :frame_a, + frame_out = frame_in) + @named x_in = RealInput() + @named y_in = RealInput() + @named phi_in = RealInput() + + @named x_out = RealOutput() + @named y_out = RealOutput() + @named phi_out = RealOutput() + + @named partial_relative_base_sensor = PartialRelativeBaseSensor() + @unpack frame_a, frame_b, frame_resolve = partial_relative_base_sensor + + systems = [ + x_in, + y_in, + phi_in, + x_out, + y_out, + phi_out, + frame_a, + frame_b, + frame_resolve, + ] + + eqs = Equation[] + + R1 = Matrix{Float64}(undef, 3, 3) + + if frame_out == frame_in + append!(eqs, [ + x_out.u ~ x_in.u, + y_out.u ~ y_in.u, + phi_out.u ~ phi_in.u, + ]) + else + if frame_in == :world + R1 = [1.0 0.0 0.0; + 0.0 1.0 0.0; + 0.0 0.0 1.0] + elseif frame_in == :frame_a + R1 = [cos(frame_a.phi) -sin(frame_a.phi) 0.0; + sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] + elseif frame_in == :frame_b + R1 = [cos(frame_b.phi) -sin(frame_b.phi) 0.0; + sin(frame_b.phi) cos(frame_b.phi) 0.0; + 0.0 0.0 1.0] + else + R1 = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0; + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] + end + + r_in = [x_in.u, y_in.u, phi_in.u] + if frame_out == :world + r_out = R1 * r_in + elseif frame_out == :frame_a + rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0.0; + sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] + r_out = transpose(rotation_matrix) * (R1 * r_in) + elseif frame_out == :frame_b + rotation_matrix = [cos(frame_b.phi) -sin(frame_b.phi) 0.0; + sin(frame_b.phi) cos(frame_b.phi) 0.0; + 0.0 0.0 1.0] + r_out = transpose(rotation_matrix) * (R1 * r_in) + else + rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0; + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] + r_out = transpose(rotation_matrix) * (R1 * r_in) + end + append!(eqs, [ + x_out.u ~ r_out[1], + y_out.u ~ r_out[2], + phi_out.u ~ r_out[3], + ]) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end From 32dfb13b17a7f5f28921a6137962f1f2d23683a5 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 14:15:07 +0300 Subject: [PATCH 39/61] Add `TransformRelativeVector` utility --- src/Mechanical/PlanarMechanics/sensors.jl | 55 +++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 7dc203e21..a5617ae7a 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -567,3 +567,58 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function TransformRelativeVector(; + name, + frame_in = :frame_a, + frame_out = frame_in) + @named x_in = RealInput() + @named y_in = RealInput() + @named phi_in = RealInput() + + @named x_out = RealOutput() + @named y_out = RealOutput() + @named phi_out = RealOutput() + + @named basic_transformb_vector = BasicTransformRelativeVector(; frame_in, frame_out) + @named partial_relative_sensor = PartialRelativeSensor() + @unpack frame_a, frame_b = partial_relative_sensor + + systems = [ + x_in, + y_in, + phi_in, + x_out, + y_out, + phi_out, + basic_transformb_vector, + frame_a, + frame_b, + ] + + eqs = [ + connect(basic_transformb_vector.frame_a, frame_a), + connect(basic_transformb_vector.frame_b, frame_b), + # out + connect(basic_transformb_vector.x_out, x_out), + connect(basic_transformb_vector.y_out, y_out), + connect(basic_transformb_vector.phi_out, phi_out), + # in + connect(basic_transformb_vector.x_in, x_in), + connect(basic_transformb_vector.y_in, y_in), + connect(basic_transformb_vector.phi_in, phi_in), + ] + + if frame_in == :frame_resolve || frame_out == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(basic_transformb_vector.frame_resolve, frame_resolve)) + end + + if !(frame_in == :frame_resolve || frame_out == :frame_resolve) + @named zero_pos = ZeroPosition() + push!(systems, zero_pos) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end From b451b6db337befa1d7db1bf871868a27d8a928f4 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 15:41:18 +0300 Subject: [PATCH 40/61] Add `RelativeVelocity` sensor --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/sensors.jl | 57 ++++++++++++++++++- test/Mechanical/planar_mechanics.jl | 28 ++++++--- 3 files changed, 77 insertions(+), 10 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 4c0778f77..564424780 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -22,6 +22,6 @@ include("components.jl") export Revolute, Prismatic include("joints.jl") -export AbsolutePosition, RelativePosition, AbsoluteVelocity +export AbsolutePosition, RelativePosition, AbsoluteVelocity, RelativeVelocity include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index a5617ae7a..4e66dbfed 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -483,7 +483,7 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end -@named function BasicTransformRelativeVector(; +@component function BasicTransformRelativeVector(; name, frame_in = :frame_a, frame_out = frame_in) @@ -622,3 +622,58 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function RelativeVelocity(; name, resolve_in_frame = :frame_a) + @named partial_rel_sensor = PartialRelativeSensor() + @unpack frame_a, frame_b = partial_rel_sensor + + @named rel_v_x = RealOutput() + @named rel_v_y = RealOutput() + @named rel_ω = RealOutput() + + @named rel_pos = RelativePosition(; resolve_in_frame = :frame_a) + @named transform_relative_vector = TransformRelativeVector(; + frame_in = :frame_a, + frame_out = resolve_in_frame) + + systems = [ + frame_a, + frame_b, + rel_v_x, + rel_v_y, + rel_ω, + rel_pos, + transform_relative_vector, + ] + eqs = [ + # connect(relativePosition.r_rel, der_r_rel.u) + # connect(der_r_rel.y, transformRelativeVector.r_in) + D(rel_pos.rel_x.u) ~ transform_relative_vector.x_in.u, + D(rel_pos.rel_y.u) ~ transform_relative_vector.y_in.u, + D(rel_pos.rel_phi.u) ~ transform_relative_vector.phi_in.u, + # connect(transformRelativeVector.r_out, v_rel) + transform_relative_vector.x_out.u ~ rel_v_x.u, + transform_relative_vector.y_out.u ~ rel_v_y.u, + transform_relative_vector.phi_out.u ~ rel_ω.u, + connect(rel_pos.frame_a, frame_a), + connect(rel_pos.frame_b, frame_b), + connect(transform_relative_vector.frame_a, frame_a), + connect(transform_relative_vector.frame_b, frame_b), + ] + + if resolve_in_frame == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(transform_relative_vector.frame_resolve, frame_resolve)) + end + + if resolve_in_frame != :frame_resolve + @named zero_pos = ZeroPosition() + push!(systems, zero_pos) + push!(eqs, + connect(transform_relative_vector.zero_pos.frame_resolve, + zero_pos.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index c1269931a..8f05a25d7 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -64,7 +64,7 @@ end @test true end -@testset "Position Sensors (two free falling bodies)" begin +@testset "Sensors (two free falling bodies)" begin m = 1 j = 1 resolve_in_frame = :world @@ -77,14 +77,20 @@ end @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) + @named rel_v_sensor1 = RelativeVelocity(; resolve_in_frame) + @named rel_v_sensor2 = RelativeVelocity(; resolve_in_frame) connections = [ connect(body1.frame, abs_pos_sensor.frame_a), connect(rel_pos_sensor1.frame_a, body1.frame), + connect(rel_v_sensor1.frame_a, body1.frame), connect(rel_pos_sensor1.frame_b, base.frame), + connect(rel_v_sensor1.frame_b, base.frame), connect(rel_pos_sensor2.frame_b, body1.frame), + connect(rel_v_sensor2.frame_b, body1.frame), connect(rel_pos_sensor2.frame_a, body2.frame), - connect(body1.frame, abs_v_sensor.frame_a), + connect(rel_v_sensor2.frame_a, body2.frame), + connect(abs_v_sensor.frame_a, body1.frame), [s ~ 0 for s in (body1.phi, body2.phi, body1.fx, body1.fy, body2.fx, body2.fy)]..., ] @@ -100,6 +106,8 @@ end abs_v_sensor, rel_pos_sensor1, rel_pos_sensor2, + rel_v_sensor1, + rel_v_sensor2, ]) sys = structural_simplify(model) @@ -114,7 +122,7 @@ end 0.5 * g * tspan[end]^2 # sensor1 is attached to body1, so the relative y-position between body1 and the base is - # equal to the y-position of body1 + # equal to the absolute y-position of body1 @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.rel_y.u][end] # the relative y-position between body1 and body2 is zero @@ -123,12 +131,16 @@ end # no displacement in the x-direction @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] - # velocity after t seconds v = g * t - @test sol[abs_v_sensor.v_y.u][end] ≈ g * tspan[end] + # velocity after t seconds v = g * t, so the relative y-velocity between body1 and the base is + # equal to the absolute y-velocity of body1 + @test sol[abs_v_sensor.v_y.u][end] ≈ -sol[rel_v_sensor1.rel_v_y.u][end] ≈ g * tspan[end] + + # the relative y-velocity between body1 and body2 is zero + @test sol[rel_v_sensor2.rel_v_y.u][end] == 0 end @testset "Measure Demo" begin - @test_nowarn @named abs_v_w = AbsoluteVelocity(; resolve_in_frame = :world) - @test_nowarn @named abs_v_fa = AbsoluteVelocity(; resolve_in_frame = :frame_a) - @test_nowarn @named abs_v_fr = AbsoluteVelocity(; resolve_in_frame = :frame_resolve) + @test_nowarn @named rel_v_w = RelativeVelocity(; resolve_in_frame = :world) + @test_nowarn @named rel_v_fa = RelativeVelocity(; resolve_in_frame = :frame_a) + @test_nowarn @named rel_v_fr = RelativeVelocity(; resolve_in_frame = :frame_resolve) end From f1031a3c3f6cdd78f7a64564ecff2e82d5a00273 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 18:02:31 +0300 Subject: [PATCH 41/61] Add `AbsoluteAccleration` sensor --- .../PlanarMechanics/PlanarMechanics.jl | 3 +- src/Mechanical/PlanarMechanics/sensors.jl | 50 +++++++++++++++++++ test/Mechanical/planar_mechanics.jl | 8 ++- 3 files changed, 59 insertions(+), 2 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 564424780..9411c710a 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -22,6 +22,7 @@ include("components.jl") export Revolute, Prismatic include("joints.jl") -export AbsolutePosition, RelativePosition, AbsoluteVelocity, RelativeVelocity +export AbsolutePosition, + RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAccleration include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 4e66dbfed..1823dabdb 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -677,3 +677,53 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function AbsoluteAccleration(; name, resolve_in_frame) + @named partial_abs_sensor = PartialAbsoluteSensor() + @unpack frame_a, = partial_abs_sensor + + @named a_x = RealOutput() + @named a_y = RealOutput() + @named α = RealOutput() + + @named pos = BasicAbsolutePosition(; resolve_in_frame = :world) + @named zero_pos = ZeroPosition() + + @named transform_absolute_vector = TransformAbsoluteVector(; + frame_in = :world, + frame_out = resolve_in_frame) + + systems = [frame_a, a_x, a_y, α, pos, zero_pos, transform_absolute_vector] + + eqs = [ + # connect(position.r, der1.u) + # connect(der1.y, der2.u) + # connect(der2.y, transformAbsoluteVector.r_in) + D(D(pos.x.u)) ~ transform_absolute_vector.x_in.u, + D(D(pos.y.u)) ~ transform_absolute_vector.y_in.u, + D(D(pos.phi.u)) ~ transform_absolute_vector.phi_in.u, + # connect(transformAbsoluteVector.r_out, a) + transform_absolute_vector.x_out.u ~ a_x.u, + transform_absolute_vector.y_out.u ~ a_y.u, + transform_absolute_vector.phi_out.u ~ α.u, + connect(pos.frame_a, frame_a), + connect(zero_pos.frame_resolve, pos.frame_resolve), + connect(transform_absolute_vector.frame_a, frame_a), + ] + + if resolve_in_frame == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(transform_absolute_vector.frame_resolve, frame_resolve)) + end + + if resolve_in_frame != :frame_resolve + @named zero_pos1 = ZeroPosition() + push!(systems, zero_pos1) + push!(eqs, + connect(transform_absolute_vector.zero_pos.frame_resolve, + zero_pos1.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 8f05a25d7..252f16d13 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -75,6 +75,7 @@ end @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) + @named abs_a_sensor = AbsoluteAccleration(; resolve_in_frame) @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) @named rel_v_sensor1 = RelativeVelocity(; resolve_in_frame) @@ -82,6 +83,8 @@ end connections = [ connect(body1.frame, abs_pos_sensor.frame_a), + connect(abs_v_sensor.frame_a, body1.frame), + connect(abs_a_sensor.frame_a, body1.frame), connect(rel_pos_sensor1.frame_a, body1.frame), connect(rel_v_sensor1.frame_a, body1.frame), connect(rel_pos_sensor1.frame_b, base.frame), @@ -90,7 +93,6 @@ end connect(rel_v_sensor2.frame_b, body1.frame), connect(rel_pos_sensor2.frame_a, body2.frame), connect(rel_v_sensor2.frame_a, body2.frame), - connect(abs_v_sensor.frame_a, body1.frame), [s ~ 0 for s in (body1.phi, body2.phi, body1.fx, body1.fy, body2.fx, body2.fy)]..., ] @@ -104,6 +106,7 @@ end base, abs_pos_sensor, abs_v_sensor, + abs_a_sensor, rel_pos_sensor1, rel_pos_sensor2, rel_v_sensor1, @@ -137,6 +140,9 @@ end # the relative y-velocity between body1 and body2 is zero @test sol[rel_v_sensor2.rel_v_y.u][end] == 0 + + # the body is under constant acclertation = g + @test all(sol[abs_a_sensor.a_y.u] .≈ g) end @testset "Measure Demo" begin From 2680ec855efc06fe6ea59cab9f5b3191a857228c Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 2 Oct 2023 18:31:30 +0300 Subject: [PATCH 42/61] Add `RelativeAcclerationSensor` --- .../PlanarMechanics/PlanarMechanics.jl | 3 +- src/Mechanical/PlanarMechanics/sensors.jl | 59 ++++++++++++++++++- test/Mechanical/planar_mechanics.jl | 23 ++++++-- 3 files changed, 77 insertions(+), 8 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 9411c710a..3ae4fd132 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -23,6 +23,7 @@ export Revolute, Prismatic include("joints.jl") export AbsolutePosition, - RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAccleration + RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAcceleration, + RelativeAcceleration include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 1823dabdb..5269494a6 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -678,7 +678,7 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end -@component function AbsoluteAccleration(; name, resolve_in_frame) +@component function AbsoluteAcceleration(; name, resolve_in_frame = :frame_a) @named partial_abs_sensor = PartialAbsoluteSensor() @unpack frame_a, = partial_abs_sensor @@ -727,3 +727,60 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +@component function RelativeAcceleration(; name, resolve_in_frame = :frame_a) + @named partial_rel_sensor = PartialRelativeSensor() + @unpack frame_a, frame_b = partial_rel_sensor + + @named rel_a_x = RealOutput() + @named rel_a_y = RealOutput() + @named rel_α = RealOutput() + + @named rel_pos = RelativePosition(; resolve_in_frame = :frame_a) + @named transform_relative_vector = TransformRelativeVector(; + frame_in = :frame_a, + frame_out = resolve_in_frame) + + systems = [ + frame_a, + frame_b, + rel_a_x, + rel_a_y, + rel_α, + rel_pos, + transform_relative_vector, + ] + + eqs = [ + # connect(der_r_rel.y, transformRelativeVector.r_in) + # connect(transformRelativeVector.r_out, der_r_rel1.u) + # connect(relativePosition.r_rel, der_r_rel.u) + D(D(rel_pos.rel_x.u)) ~ transform_relative_vector.x_in.u, + D(D(rel_pos.rel_y.u)) ~ transform_relative_vector.y_in.u, + D(D(rel_pos.rel_phi.u)) ~ transform_relative_vector.phi_in.u, + # connect(der_r_rel1.y, a_rel), + transform_relative_vector.x_out.u ~ rel_a_x.u, + transform_relative_vector.y_out.u ~ rel_a_y.u, + transform_relative_vector.phi_out.u ~ rel_α.u, + connect(rel_pos.frame_a, frame_a), + connect(rel_pos.frame_b, frame_b), + connect(transform_relative_vector.frame_a, frame_a), + connect(transform_relative_vector.frame_b, frame_b), + ] + + if resolve_in_frame == :frame_resolve + @named frame_resolve = FrameResolve() + push!(systems, frame_resolve) + push!(eqs, connect(transform_relative_vector.frame_resolve, frame_resolve)) + end + + if resolve_in_frame != :frame_resolve + @named zero_pos = ZeroPosition() + push!(systems, zero_pos) + push!(eqs, + connect(transform_relative_vector.zero_pos.frame_resolve, + zero_pos.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 252f16d13..17d0db618 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -75,11 +75,13 @@ end @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) - @named abs_a_sensor = AbsoluteAccleration(; resolve_in_frame) + @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame) @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) @named rel_v_sensor1 = RelativeVelocity(; resolve_in_frame) @named rel_v_sensor2 = RelativeVelocity(; resolve_in_frame) + @named rel_a_sensor1 = RelativeAcceleration(; resolve_in_frame) + @named rel_a_sensor2 = RelativeAcceleration(; resolve_in_frame) connections = [ connect(body1.frame, abs_pos_sensor.frame_a), @@ -93,6 +95,10 @@ end connect(rel_v_sensor2.frame_b, body1.frame), connect(rel_pos_sensor2.frame_a, body2.frame), connect(rel_v_sensor2.frame_a, body2.frame), + connect(rel_a_sensor1.frame_a, body1.frame), + connect(rel_a_sensor1.frame_b, base.frame), + connect(rel_a_sensor2.frame_a, body1.frame), + connect(rel_a_sensor2.frame_b, body2.frame), [s ~ 0 for s in (body1.phi, body2.phi, body1.fx, body1.fy, body2.fx, body2.fy)]..., ] @@ -111,6 +117,8 @@ end rel_pos_sensor2, rel_v_sensor1, rel_v_sensor2, + rel_a_sensor1, + rel_a_sensor2, ]) sys = structural_simplify(model) @@ -143,10 +151,13 @@ end # the body is under constant acclertation = g @test all(sol[abs_a_sensor.a_y.u] .≈ g) -end -@testset "Measure Demo" begin - @test_nowarn @named rel_v_w = RelativeVelocity(; resolve_in_frame = :world) - @test_nowarn @named rel_v_fa = RelativeVelocity(; resolve_in_frame = :frame_a) - @test_nowarn @named rel_v_fr = RelativeVelocity(; resolve_in_frame = :frame_resolve) + # the relative y-accleration between body1 and the base is + # equal to the absolute y-accleration of body1 + @test sol[abs_a_sensor.a_y.u][end] ≈ -sol[rel_a_sensor1.rel_a_y.u][end] + + # the relative y-accleration between body1 and body2 is zero + @test sol[rel_a_sensor2.rel_a_y.u][end] == 0 end + +@testset "Measure Demo" begin end From b2f525dc5e644f648cc0a6deab4c92dde9dacd31 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 5 Oct 2023 15:40:35 +0300 Subject: [PATCH 43/61] Simplify `Body` and `FixedTranslation` components --- src/Mechanical/PlanarMechanics/components.jl | 30 +++++++------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index d5bdf364e..9f48d833f 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -59,7 +59,7 @@ Body component with mass and inertia - `frame`: 2-dim. Coordinate system """ -@component function Body(; name, m, j, r = nothing, gy = -9.807) +@component function Body(; name, m, j, rx = 0, ry = 0, gy = -9.807) @named frame = Frame() pars = @parameters begin m = m @@ -70,8 +70,8 @@ Body component with mass and inertia vars = @variables begin fx(t) = 0 fy(t) = 0 - rx(t) = 0 - ry(t) = 0 + rx(t) = rx + ry(t) = ry vx(t) = 0 vy(t) = 0 ax(t) = 0 @@ -101,11 +101,6 @@ Body component with mass and inertia j * α ~ frame.j, ] - if r !== nothing - rx = r[1] - ry = r[2] - end - return compose(ODESystem(eqs, t, vars, pars; name = name), frame) end @@ -138,25 +133,20 @@ A fixed translation between two components (rigid rod) ] end - @variables begin - r0x(t), [description = "x-length of the rod resolved w.r.t to inertal frame"] - r0y(t), [description = "y-length of the rod resolved w.r.t to inertal frame"] - cos_phi(t), [description = "cos(phi)"] - sin_phi(t), [description = "sin(phi)"] + begin + R = [cos(frame_a.phi) -sin(frame_a.phi); + sin(frame_a.phi) cos(frame_a.phi)] + r0 = R * [rx, ry] end @equations begin # rigidly connect positions - frame_a.x + rx ~ frame_b.x - frame_a.y + ry ~ frame_b.y + frame_a.x + r0[1] ~ frame_b.x + frame_a.y + r0[2] ~ frame_b.y frame_a.phi ~ frame_b.phi # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 - cos_phi ~ cos(frame_a.phi) - sin_phi ~ sin(frame_a.phi) - r0x ~ cos_phi * rx - sin_phi * ry - r0y ~ sin_phi * rx + cos_phi * rx - frame_a.j + frame_b.j + r0x * (frame_b.fy - frame_a.fy) - r0y * (frame_b.fx - frame_a.fx) ~ 0 + frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 end end From faf51633a528fdda8161cd25cb404fcd24552915 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 5 Oct 2023 15:41:49 +0300 Subject: [PATCH 44/61] Fix a typo --- src/Mechanical/PlanarMechanics/sensors.jl | 24 +++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 5269494a6..283876e02 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -159,7 +159,7 @@ Measure absolute position and orientation of the origin of frame connector - `resolve_in_frame`: Frame in which output x, y, phi is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) """ @component function AbsolutePosition(; name, resolve_in_frame = :frame_a) - @named pos = BasicAbsolutePosition() + @named pos = BasicAbsolutePosition(; resolve_in_frame) @named partial_abs_sensor = PartialAbsoluteSensor() @unpack frame_a, = partial_abs_sensor @@ -402,33 +402,33 @@ end @named x_out = RealOutput() @named y_out = RealOutput() @named phi_out = RealOutput() - @named basic_transformb_vector = BasicTransformAbsoluteVector(; frame_in, frame_out) + @named basic_transform_vector = BasicTransformAbsoluteVector(; frame_in, frame_out) - systems = [frame_a, x_in, y_in, phi_in, x_out, y_out, phi_out, basic_transformb_vector] + systems = [frame_a, x_in, y_in, phi_in, x_out, y_out, phi_out, basic_transform_vector] eqs = [ - connect(basic_transformb_vector.frame_a, frame_a), + connect(basic_transform_vector.frame_a, frame_a), # out - connect(basic_transformb_vector.x_out, x_out), - connect(basic_transformb_vector.y_out, y_out), - connect(basic_transformb_vector.phi_out, phi_out), + connect(basic_transform_vector.x_out, x_out), + connect(basic_transform_vector.y_out, y_out), + connect(basic_transform_vector.phi_out, phi_out), # in - connect(basic_transformb_vector.x_in, x_in), - connect(basic_transformb_vector.y_in, y_in), - connect(basic_transformb_vector.phi_in, phi_in), + connect(basic_transform_vector.x_in, x_in), + connect(basic_transform_vector.y_in, y_in), + connect(basic_transform_vector.phi_in, phi_in), ] if frame_in == :frame_resolve || frame_out == :frame_resolve @named frame_resolve = FrameResolve() push!(systems, frame_resolve) - push!(eqs, connect(basic_transformb_vector.frame_resolve, frame_resolve)) + push!(eqs, connect(basic_transform_vector.frame_resolve, frame_resolve)) end if !(frame_in == :frame_resolve || frame_out == :frame_resolve) @named zero_pos = ZeroPosition() push!(systems, zero_pos) push!(eqs, - connect(zero_pos.frame_resolve, basic_transformb_vector.frame_resolve)) + connect(zero_pos.frame_resolve, basic_transform_vector.frame_resolve)) end return compose(ODESystem(eqs, t, [], []; name = name), systems...) From 1393610fb6769e5052f28b56a44c19d171a14d12 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 5 Oct 2023 15:43:28 +0300 Subject: [PATCH 45/61] Refactor `Revolute` --- src/Mechanical/PlanarMechanics/joints.jl | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 9a8eb3bb2..210ef7532 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -27,13 +27,6 @@ A revolute joint @named fixed = Rotational.Fixed() systems = [frame_a, frame_b, fixed] - if use_flange - @named flange_a = Rotational.Flange(; phi, tau) - push!(systems, flange_a) - @named support = Rotational.Support() - push!(systems, support) - end - vars = @variables begin phi(t) = phi ω(t) = ω @@ -57,6 +50,10 @@ A revolute joint ] if use_flange + @named flange_a = Rotational.Flange(; phi, tau) + push!(systems, flange_a) + @named support = Rotational.Support() + push!(systems, support) push!(eqs, connect(fixed.flange, support)) else # actutation torque From fcc37079d44eb3666782738551691a3e46b911b8 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 5 Oct 2023 21:35:34 +0300 Subject: [PATCH 46/61] Add `AbsoluteAccCentrifugal` test (w/o sensors) --- test/Mechanical/planar_mechanics.jl | 53 ++++++++++++++++++++++++++--- 1 file changed, 48 insertions(+), 5 deletions(-) diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 17d0db618..9ffdc0256 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -48,12 +48,8 @@ end [], systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) - sol = solve(prob, Rodas5P()) - # phi and omega for the pendulum body - @test length(states(sys)) == 2 + @test length(states(sys)) == 7 end @testset "Prismatic" begin @@ -64,6 +60,53 @@ end @test true end +@testset "AbsoluteAccCentrifugal" begin + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanicsTest/Sensors.mo#L221-L332 + m = 1 + j = 0.1 + ω = 10 + resolve_in_frame = :world + + # components + @named body = Body(; m, j, gy = 0.0) + @named fixed_translation = FixedTranslation(; rx = 10.0, ry = 0.0) + @named fixed = Fixed() + @named revolute = Revolute(ω = ω) + + eqs = [ + connect(fixed_translation.frame_b, body.frame), + connect(fixed.frame, revolute.frame_a), + connect(revolute.frame_b, fixed_translation.frame_a), + ] + + @named model = ODESystem(eqs, + t, + [], + [], + systems = [ + body, + fixed_translation, + fixed, + revolute, + ]) + sys = structural_simplify(model) + prob = ODEProblem(sys, [0.0, 10.0, 0.0], tspan, []; jac = true) + sol = solve(prob, Rodas5P()) + + # phi + @test sol[body.phi][end] ≈ tspan[end] * ω + @test all(sol[body.ω] .≈ ω) + + test_points = [i / ω for i in 0:0.1:10] + # instantaneous linear velocity + v_singal(t) = -ω^2 * sin.(ω .* t) + @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = body.vx)) + + # instantaneous linear acceleration + a_singal(t) = -ω^3 * cos.(ω .* t) + @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) +end + @testset "Sensors (two free falling bodies)" begin m = 1 j = 1 From 61f19a3b83ef02019a28b3df27acb8e9d933edb3 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 6 Oct 2023 15:32:07 +0300 Subject: [PATCH 47/61] Add `AbsolutePoision` sensor to `AbsoluteAccCentrifugal` test --- src/Mechanical/PlanarMechanics/components.jl | 4 ++-- src/Mechanical/PlanarMechanics/joints.jl | 19 +++++++++++++------ test/Mechanical/planar_mechanics.jl | 18 +++++++++++++----- 3 files changed, 28 insertions(+), 13 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 9f48d833f..235226e30 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -123,11 +123,11 @@ A fixed translation between two components (rigid rod) @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @parameters begin - rx, + rx = 0, [ description = "Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0", ] - ry, + ry = 0, [ description = "Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0", ] diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 210ef7532..ee36d39f1 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -21,22 +21,28 @@ A revolute joint - `support` [Support](@ref) if `use_flange == true` """ -@component function Revolute(; name, phi = 0.0, ω = 0.0, tau = 0.0, use_flange = false) +@component function Revolute(; + name, + constant_phi = nothing, + constant_ω = nothing, + constat_tau = nothing, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() systems = [frame_a, frame_b, fixed] vars = @variables begin - phi(t) = phi - ω(t) = ω + phi(t) = 0.0 + ω(t) = 0.0 α(t) = 0.0 - j(t) = tau + j(t) = 0.0 end eqs = [ - ω ~ D(phi), - α ~ D(ω), + phi ~ ifelse(constant_phi === nothing, phi, constant_phi), + ω ~ ifelse(constant_ω === nothing, D(phi), constant_ω), + α ~ ifelse(constant_ω === nothing, D(ω), 0.0), # rigidly connect positions frame_a.x ~ frame_b.x, frame_a.y ~ frame_b.y, @@ -46,6 +52,7 @@ A revolute joint frame_a.fy + frame_b.fy ~ 0, # balance torques frame_a.j + frame_b.j ~ 0, + j ~ ifelse(constat_tau === nothing, j, constat_tau), frame_a.j ~ j, ] diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 9ffdc0256..6e224b320 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -34,7 +34,7 @@ end @named ceiling = Fixed() @named rod = FixedTranslation(rx = 1.0, ry = 0.0) @named body = Body(m = 1, j = 0.1) - @named revolute = Revolute(phi = 0.0, ω = 0.0) + @named revolute = Revolute() connections = [ connect(ceiling.frame, revolute.frame_a), @@ -65,18 +65,25 @@ end m = 1 j = 0.1 ω = 10 - resolve_in_frame = :world # components @named body = Body(; m, j, gy = 0.0) @named fixed_translation = FixedTranslation(; rx = 10.0, ry = 0.0) @named fixed = Fixed() - @named revolute = Revolute(ω = ω) + @named revolute = Revolute(constant_ω = ω) + + # sensors + @named abs_pos_sensor = AbsolutePosition() eqs = [ - connect(fixed_translation.frame_b, body.frame), connect(fixed.frame, revolute.frame_a), connect(revolute.frame_b, fixed_translation.frame_a), + connect(fixed_translation.frame_b, body.frame), + connect(abs_pos_sensor.frame_a, body.frame), + # TODO: the following equations shouldn't be necessary + body.ω ~ revolute.ω, + fixed.frame.fy ~ -body.fy, + fixed.frame.fx ~ -body.fx, ] @named model = ODESystem(eqs, @@ -88,9 +95,10 @@ end fixed_translation, fixed, revolute, + abs_pos_sensor, ]) sys = structural_simplify(model) - prob = ODEProblem(sys, [0.0, 10.0, 0.0], tspan, []; jac = true) + prob = ODEProblem(sys, [0.0], tspan, []; jac = true) sol = solve(prob, Rodas5P()) # phi From 79b24897f8a232d1534132d5fb299cea6ce5529d Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 8 Oct 2023 12:23:17 +0300 Subject: [PATCH 48/61] fine-grain sensor connections --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/sensors.jl | 21 ++++++++++ test/Mechanical/planar_mechanics.jl | 42 +++++++++---------- 3 files changed, 41 insertions(+), 24 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 3ae4fd132..2a5fee518 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -24,6 +24,6 @@ include("joints.jl") export AbsolutePosition, RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAcceleration, - RelativeAcceleration + RelativeAcceleration, connect_sensor include("sensors.jl") end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 283876e02..369b80930 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -138,6 +138,9 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but x.u ~ r[1], y.u ~ r[2], phi.u ~ r[3], + frame_a.fx ~ 0, + frame_a.fy ~ 0, + frame_a.j ~ 0, ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -244,6 +247,12 @@ Measure relative position and orientation between the origins of two frame conne rel_x.u ~ r[1], rel_y.u ~ r[2], rel_phi.u ~ r[3], + frame_a.fx ~ 0, + frame_a.fy ~ 0, + frame_a.j ~ 0, + frame_b.fx ~ 0, + frame_b.fy ~ 0, + frame_b.j ~ 0, ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -709,6 +718,9 @@ end connect(pos.frame_a, frame_a), connect(zero_pos.frame_resolve, pos.frame_resolve), connect(transform_absolute_vector.frame_a, frame_a), + frame_a.fx ~ 0, + frame_a.fy ~ 0, + frame_a.j ~ 0, ] if resolve_in_frame == :frame_resolve @@ -784,3 +796,12 @@ end return compose(ODESystem(eqs, t, [], []; name = name), systems...) end + +function connect_sensor(component_frame, sensor_frame) + # TODO: make this an override of the `connect` method + return [ + component_frame.x ~ sensor_frame.x, + component_frame.y ~ sensor_frame.y, + component_frame.phi ~ sensor_frame.phi, + ] +end \ No newline at end of file diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 6e224b320..cd575ff86 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -79,11 +79,7 @@ end connect(fixed.frame, revolute.frame_a), connect(revolute.frame_b, fixed_translation.frame_a), connect(fixed_translation.frame_b, body.frame), - connect(abs_pos_sensor.frame_a, body.frame), - # TODO: the following equations shouldn't be necessary - body.ω ~ revolute.ω, - fixed.frame.fy ~ -body.fy, - fixed.frame.fx ~ -body.fx, + connect_sensor(abs_pos_sensor.frame_a, body.frame)..., ] @named model = ODESystem(eqs, @@ -98,7 +94,8 @@ end abs_pos_sensor, ]) sys = structural_simplify(model) - prob = ODEProblem(sys, [0.0], tspan, []; jac = true) + u0 = [0.0, ω, 0.0] + prob = ODEProblem(sys, u0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) # phi @@ -135,22 +132,21 @@ end @named rel_a_sensor2 = RelativeAcceleration(; resolve_in_frame) connections = [ - connect(body1.frame, abs_pos_sensor.frame_a), - connect(abs_v_sensor.frame_a, body1.frame), - connect(abs_a_sensor.frame_a, body1.frame), - connect(rel_pos_sensor1.frame_a, body1.frame), - connect(rel_v_sensor1.frame_a, body1.frame), - connect(rel_pos_sensor1.frame_b, base.frame), - connect(rel_v_sensor1.frame_b, base.frame), - connect(rel_pos_sensor2.frame_b, body1.frame), - connect(rel_v_sensor2.frame_b, body1.frame), - connect(rel_pos_sensor2.frame_a, body2.frame), - connect(rel_v_sensor2.frame_a, body2.frame), - connect(rel_a_sensor1.frame_a, body1.frame), - connect(rel_a_sensor1.frame_b, base.frame), - connect(rel_a_sensor2.frame_a, body1.frame), - connect(rel_a_sensor2.frame_b, body2.frame), - [s ~ 0 for s in (body1.phi, body2.phi, body1.fx, body1.fy, body2.fx, body2.fy)]..., + connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., + connect_sensor(body1.frame, abs_v_sensor.frame_a)..., + connect_sensor(body1.frame, abs_a_sensor.frame_a)..., + connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., + connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., + connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., + connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., + connect_sensor(base.frame, rel_v_sensor1.frame_a)..., + connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., + connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., + connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., + connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., + connect_sensor(base.frame, rel_a_sensor1.frame_b)..., + connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., + connect_sensor(body2.frame, rel_a_sensor2.frame_b)..., ] @named model = ODESystem(connections, @@ -195,7 +191,7 @@ end # velocity after t seconds v = g * t, so the relative y-velocity between body1 and the base is # equal to the absolute y-velocity of body1 - @test sol[abs_v_sensor.v_y.u][end] ≈ -sol[rel_v_sensor1.rel_v_y.u][end] ≈ g * tspan[end] + @test sol[abs_v_sensor.v_y.u][end] ≈ sol[rel_v_sensor1.rel_v_y.u][end] ≈ g * tspan[end] # the relative y-velocity between body1 and body2 is zero @test sol[rel_v_sensor2.rel_v_y.u][end] == 0 From 8a412250a130d893c11f515adcf4c351a2a52abd Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 8 Oct 2023 14:27:38 +0300 Subject: [PATCH 49/61] Test centrifugal velocity --- src/Mechanical/PlanarMechanics/sensors.jl | 3 --- test/Mechanical/planar_mechanics.jl | 10 ++++++---- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index 369b80930..e190d7a06 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -718,9 +718,6 @@ end connect(pos.frame_a, frame_a), connect(zero_pos.frame_resolve, pos.frame_resolve), connect(transform_absolute_vector.frame_a, frame_a), - frame_a.fx ~ 0, - frame_a.fy ~ 0, - frame_a.j ~ 0, ] if resolve_in_frame == :frame_resolve diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index cd575ff86..1cf62e1cf 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -65,6 +65,7 @@ end m = 1 j = 0.1 ω = 10 + resolve_in_frame = :world # components @named body = Body(; m, j, gy = 0.0) @@ -73,13 +74,13 @@ end @named revolute = Revolute(constant_ω = ω) # sensors - @named abs_pos_sensor = AbsolutePosition() + @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) eqs = [ connect(fixed.frame, revolute.frame_a), connect(revolute.frame_b, fixed_translation.frame_a), connect(fixed_translation.frame_b, body.frame), - connect_sensor(abs_pos_sensor.frame_a, body.frame)..., + connect_sensor(body.frame, abs_v_sensor.frame_a)..., ] @named model = ODESystem(eqs, @@ -91,7 +92,7 @@ end fixed_translation, fixed, revolute, - abs_pos_sensor, + abs_v_sensor, ]) sys = structural_simplify(model) u0 = [0.0, ω, 0.0] @@ -103,9 +104,10 @@ end @test all(sol[body.ω] .≈ ω) test_points = [i / ω for i in 0:0.1:10] + # instantaneous linear velocity v_singal(t) = -ω^2 * sin.(ω .* t) - @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = body.vx)) + @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) # instantaneous linear acceleration a_singal(t) = -ω^3 * cos.(ω .* t) From f03388be6b1f549ca6b84d18dc4aa69feb68486e Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Sun, 8 Oct 2023 20:02:02 +0300 Subject: [PATCH 50/61] Refactor `Prismatic` --- src/Mechanical/PlanarMechanics/joints.jl | 60 ++++++++++-------------- test/Mechanical/planar_mechanics.jl | 5 +- 2 files changed, 27 insertions(+), 38 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index ee36d39f1..a34a34066 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -78,25 +78,17 @@ end A prismatic joint # parameters - - `rx`: [m] x-direction of the rod wrt. body system at phi=0 - - `ry`: [m] y-direction of the rod wrt. body system at phi=0 - - `ex`: [m] x-component of unit vector in direction of r - - `ey`: [m] y-component of unit vector in direction of r - - `f`: [N] Force in direction of elongation - - `s`: [m] Elongation of the joint" + - `x`: [m] x-direction of the rod wrt. body system at phi=0 + - `y`: [m] y-direction of the rod wrt. body system at phi=0 + - `constant_f`: [N] Constant force in direction of elongation + - `constant_s`: [m] Constant elongation of the joint" - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" # states - `s(t)`: [m] Elongation of the joint - `v(t)`: [m/s] Velocity of elongation - `a(t)`: [m/s²] Acceleration of elongation - - `e0x(t)`: [m] x-component of unit vector resolved w.r.t inertial frame - - `e0y(t)`: [m] y-component of unit vector resolved w.r.t inertial frame - - `r0x(t)`: [m] x-component of the rod resolved w.r.t to inertal frame - - `r0y(t)`: [m] y-length of the rod resolved w.r.t to inertal frame - - `cos_phi(t)`: [degree] cos(phi) - - `sin_phi(t)`: [degree] sin(phi) - + - `f(t)`: [N] Force in direction of elongation # Connectors - `frame_a` [Frame](@ref) @@ -105,14 +97,20 @@ A prismatic joint - `flange_a` [Flange](@ref) if `use_flange == true` - `support` [Support](@ref) if `use_flange == true` """ -@component function Prismatic(; name, rx, ry, ex, ey, f = 0, s = 0, use_flange = false) +@component function Prismatic(; + name, + x, + y, + constant_f = 0, + constant_s = 0, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = TranslationalModelica.Support() systems = [frame_a, frame_b, fixed] if use_flange - @named flange_a = TranslationalModelica.Flange(; f, s) + @named flange_a = TranslationalModelica.Flange(; f = constant_f, constant_s) push!(systems, flange_a) @named support = TranslationalModelica.Support() push!(systems, support) @@ -122,40 +120,34 @@ A prismatic joint s(t) = 0.0 v(t) = 0.0 a(t) = 0.0 - e0x(t) - e0y(t) - r0x(t) - r0y(t) - cos_phi(t) - sin_phi(t) + f(t) = 0.0 end + R = [cos(frame_a.phi) -sin(frame_a.phi); + sin(frame_a.phi) cos(frame_a.phi)] + e0 = R * [x, y] + r0 = e0 * s + eqs = [ + ifelse(constant_s === nothing, s ~ s, s ~ constant_s), + ifelse(constant_f === nothing, f ~ f, f ~ constant_f), v ~ D(s), a ~ D(v), # rigidly connect positions - frame_a.x + rx ~ frame_b.x, - frame_a.y + ry ~ frame_b.y, + frame_a.x + r0[1] ~ frame_b.x, + frame_a.y + r0[2] ~ frame_b.y, frame_a.phi ~ frame_b.phi, - # balance forces frame_a.fx + frame_b.fx ~ 0, frame_a.fy + frame_b.fy ~ 0, - # balance torques - cos_phi ~ cos(frame_a.phi), - sin_phi ~ sin(frame_a.phi), - e0x ~ cos_phi * ex - sin_phi * ey, - e0y ~ sin_phi * ex + cos_phi * ey, - r0x ~ e0x * s, - r0y ~ e0y * s, - frame_a.j + frame_b.j + r0x * (frame_b.fy - frame_a.fy) - r0y * (frame_b.fx - frame_a.fx) ~ 0, - frame_a.fx * e0y - frame_a.fy * e0x ~ f, + frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0, + e0[1] * frame_a.fx + e0[2] * frame_a.fy ~ f, ] if use_flange push!(eqs, connect(fixed.flange, support)) else # actutation torque - push!(eqs, f ~ 0) + push!(eqs, constant_f ~ 0) end pars = [] diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 1cf62e1cf..27f79cc87 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -53,11 +53,8 @@ end end @testset "Prismatic" begin - r = [1.0, 0.0] - e = r / sqrt(r' * r) - @named prismatic = Prismatic(rx = r[1], ry = r[2], ex = e[1], ey = e[2]) # just testing instantiation - @test true + @test_nowarn @named prismatic = Prismatic(x = 1.0, y = 0.0) end @testset "AbsoluteAccCentrifugal" begin From f454f1b4aba3d7410f8c834f408f25774913b043 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 27 Nov 2023 00:03:22 +0200 Subject: [PATCH 51/61] Fix Pendulum model, and break `AbsoluteAccCentrifugal` while doing so - There is something wrong with the `Revoulte` model and I can't figure it --- src/Mechanical/PlanarMechanics/components.jl | 14 ++++++++--- src/Mechanical/PlanarMechanics/joints.jl | 24 +++++++++--------- src/Mechanical/PlanarMechanics/sensors.jl | 26 ++++++++++---------- test/Mechanical/planar_mechanics.jl | 13 ++++++---- 4 files changed, 43 insertions(+), 34 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 235226e30..e8ce5280b 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -12,6 +12,8 @@ Frame fixed in the planar world frame at a given position and orientation # Connectors: - `frame: 2-dim. Coordinate system + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo """ @mtkmodel Fixed begin @parameters begin @@ -58,6 +60,8 @@ Body component with mass and inertia # Connectors: - `frame`: 2-dim. Coordinate system + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ @component function Body(; name, m, j, rx = 0, ry = 0, gy = -9.807) @named frame = Frame() @@ -115,9 +119,11 @@ A fixed translation between two components (rigid rod) - `ry`: [m] Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0 # Connectors: - + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/FixedTranslation.mo """ @mtkmodel FixedTranslation begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -141,12 +147,12 @@ A fixed translation between two components (rigid rod) @equations begin # rigidly connect positions - frame_a.x + r0[1] ~ frame_b.x - frame_a.y + r0[2] ~ frame_b.y + frame_a.x + rx ~ frame_b.x + frame_a.y + ry ~ frame_b.y frame_a.phi ~ frame_b.phi # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 - frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 + frame_a.j + frame_b.j + r0' * [frame_b.fy, -frame_b.fx] ~ 0 end end diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index a34a34066..8d5f5f97d 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -19,14 +19,14 @@ A revolute joint - `fixed` [Fixed](@ref) if `use_flange == false` - `flange_a` [Flange](@ref) if `use_flange == true` - `support` [Support](@ref) if `use_flange == true` - +https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Joints/Revolute.mo """ @component function Revolute(; - name, - constant_phi = nothing, - constant_ω = nothing, - constat_tau = nothing, - use_flange = false) + name, + constant_phi = nothing, + constant_ω = nothing, + constat_tau = nothing, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() @@ -98,12 +98,12 @@ A prismatic joint - `support` [Support](@ref) if `use_flange == true` """ @component function Prismatic(; - name, - x, - y, - constant_f = 0, - constant_s = 0, - use_flange = false) + name, + x, + y, + constant_f = 0, + constant_s = 0, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = TranslationalModelica.Support() diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index e190d7a06..ac17a272e 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -309,9 +309,9 @@ Measure relative position and orientation between the origins of two frame conne end @component function BasicTransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -399,9 +399,9 @@ end end @component function TransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named frame_a = Frame() @named x_in = RealInput() @@ -493,9 +493,9 @@ end end @component function BasicTransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -578,9 +578,9 @@ end end @component function TransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -801,4 +801,4 @@ function connect_sensor(component_frame, sensor_frame) component_frame.y ~ sensor_frame.y, component_frame.phi ~ sensor_frame.phi, ] -end \ No newline at end of file +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 27f79cc87..bfa56595b 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -48,8 +48,11 @@ end [], systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) - - @test length(states(sys)) == 7 + @test length(states(sys)) == 2 + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) end @testset "Prismatic" begin @@ -92,7 +95,7 @@ end abs_v_sensor, ]) sys = structural_simplify(model) - u0 = [0.0, ω, 0.0] + u0 = [0.0, ω] prob = ODEProblem(sys, u0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) @@ -104,11 +107,11 @@ end # instantaneous linear velocity v_singal(t) = -ω^2 * sin.(ω .* t) - @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) + @test_broken all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) # instantaneous linear acceleration a_singal(t) = -ω^3 * cos.(ω .* t) - @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) + @test_broken all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) end @testset "Sensors (two free falling bodies)" begin From 3a9340b96b0844137cefa0c4ad8eb1d258041609 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Mon, 27 Nov 2023 10:15:18 +0200 Subject: [PATCH 52/61] Revert "Fix Pendulum model, and break `AbsoluteAccCentrifugal` while doing so" This reverts commit f454f1b4aba3d7410f8c834f408f25774913b043. --- src/Mechanical/PlanarMechanics/components.jl | 14 +++-------- src/Mechanical/PlanarMechanics/joints.jl | 24 +++++++++--------- src/Mechanical/PlanarMechanics/sensors.jl | 26 ++++++++++---------- test/Mechanical/planar_mechanics.jl | 13 ++++------ 4 files changed, 34 insertions(+), 43 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index e8ce5280b..235226e30 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -12,8 +12,6 @@ Frame fixed in the planar world frame at a given position and orientation # Connectors: - `frame: 2-dim. Coordinate system - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo """ @mtkmodel Fixed begin @parameters begin @@ -60,8 +58,6 @@ Body component with mass and inertia # Connectors: - `frame`: 2-dim. Coordinate system - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ @component function Body(; name, m, j, rx = 0, ry = 0, gy = -9.807) @named frame = Frame() @@ -119,11 +115,9 @@ A fixed translation between two components (rigid rod) - `ry`: [m] Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0 # Connectors: - + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/FixedTranslation.mo """ @mtkmodel FixedTranslation begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -147,12 +141,12 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 @equations begin # rigidly connect positions - frame_a.x + rx ~ frame_b.x - frame_a.y + ry ~ frame_b.y + frame_a.x + r0[1] ~ frame_b.x + frame_a.y + r0[2] ~ frame_b.y frame_a.phi ~ frame_b.phi # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 - frame_a.j + frame_b.j + r0' * [frame_b.fy, -frame_b.fx] ~ 0 + frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 end end diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 8d5f5f97d..a34a34066 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -19,14 +19,14 @@ A revolute joint - `fixed` [Fixed](@ref) if `use_flange == false` - `flange_a` [Flange](@ref) if `use_flange == true` - `support` [Support](@ref) if `use_flange == true` -https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Joints/Revolute.mo + """ @component function Revolute(; - name, - constant_phi = nothing, - constant_ω = nothing, - constat_tau = nothing, - use_flange = false) + name, + constant_phi = nothing, + constant_ω = nothing, + constat_tau = nothing, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() @@ -98,12 +98,12 @@ A prismatic joint - `support` [Support](@ref) if `use_flange == true` """ @component function Prismatic(; - name, - x, - y, - constant_f = 0, - constant_s = 0, - use_flange = false) + name, + x, + y, + constant_f = 0, + constant_s = 0, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = TranslationalModelica.Support() diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index ac17a272e..e190d7a06 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -309,9 +309,9 @@ Measure relative position and orientation between the origins of two frame conne end @component function BasicTransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -399,9 +399,9 @@ end end @component function TransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named frame_a = Frame() @named x_in = RealInput() @@ -493,9 +493,9 @@ end end @component function BasicTransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -578,9 +578,9 @@ end end @component function TransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -801,4 +801,4 @@ function connect_sensor(component_frame, sensor_frame) component_frame.y ~ sensor_frame.y, component_frame.phi ~ sensor_frame.phi, ] -end +end \ No newline at end of file diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index bfa56595b..27f79cc87 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -48,11 +48,8 @@ end [], systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) - @test length(states(sys)) == 2 - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []) - sol = solve(prob, Rodas5P()) - @test SciMLBase.successful_retcode(sol) + + @test length(states(sys)) == 7 end @testset "Prismatic" begin @@ -95,7 +92,7 @@ end abs_v_sensor, ]) sys = structural_simplify(model) - u0 = [0.0, ω] + u0 = [0.0, ω, 0.0] prob = ODEProblem(sys, u0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) @@ -107,11 +104,11 @@ end # instantaneous linear velocity v_singal(t) = -ω^2 * sin.(ω .* t) - @test_broken all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) + @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) # instantaneous linear acceleration a_singal(t) = -ω^3 * cos.(ω .* t) - @test_broken all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) + @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) end @testset "Sensors (two free falling bodies)" begin From 3b0bf6403032bda37dfedae6a2356e06d68cb47c Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Wed, 29 Nov 2023 17:22:56 +0200 Subject: [PATCH 53/61] Add `SpringDamper` component --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/components.jl | 63 +++++++++++++++++++ test/Mechanical/planar_mechanics.jl | 37 +++++++++++ 3 files changed, 101 insertions(+), 1 deletion(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index 2a5fee518..a2ee09318 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -16,7 +16,7 @@ D = Differential(t) export Frame, FrameResolve, PartialTwoFrames, ZeroPosition include("utils.jl") -export Fixed, Body, FixedTranslation +export Fixed, Body, FixedTranslation, SpringDamper include("components.jl") export Revolute, Prismatic diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 235226e30..d20ec8cf8 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -150,3 +150,66 @@ A fixed translation between two components (rigid rod) frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 end end + +""" + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/SpringDamper.mo#L154C20-L154C20 +""" +@mtkmodel SpringDamper begin + @extend frame_a, frame_b = partial_frames = PartialTwoFrames() + + @parameters begin + c_x = 1, [description = "Spring constant in x dir"] + c_y = 1, [description = "Spring constant in y dir"] + c_phi = 1.0e5, [description = "Spring constant in phi dir"] + d_x = 1, [description = "Damping constant in x dir"] + d_y = 1, [description = "Damping constant in y dir"] + d_phi = 1, [description = "Damping constant in phi dir"] + s_relx0 = 0, [description = "Unstretched spring length"] + s_rely0 = 0, [description = "Unstretched spring length"] + phi_rel0 = 0, [description = "Unstretched spring length"] + s_small = 1.e-10, + [ + description = "Prevent zero-division if distance between frame_a and frame_b is zero", + ] + end + + @variables begin + v_relx(t) + v_rely(t) + ω_rel(t) = 0 + s_relx(t) + s_rely(t) + phi_rel(t) = 0 + f_x(t) + f_y(t) + j(t) + end + + begin + r_rel_0 = [s_relx, s_rely, 0] + l = sqrt(r_rel_0' * r_rel_0) + e_rel_0 = r_rel_0 / max(l, s_small) + end + + @equations begin + s_relx ~ frame_b.x - frame_a.x + s_rely ~ frame_b.y - frame_a.y + phi_rel ~ frame_b.phi - frame_a.phi + v_relx ~ D(s_relx) + v_rely ~ D(s_rely) + ω_rel ~ D(phi_rel) + + j ~ c_phi * (phi_rel - phi_rel0) + d_phi * ω_rel + frame_a.j ~ -j + frame_b.j ~ j + f_x ~ c_x * (s_relx - s_relx0) + d_x * v_relx + f_y ~ c_y * (s_rely - s_rely0) + d_y * v_rely + frame_a.fx ~ -f_x + frame_b.fx ~ f_x + frame_a.fy ~ -f_y + frame_b.fy ~ f_y + + # lossPower ~ d_x * v_relx * v_relx + d_y * v_rely * v_rely + end +end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 27f79cc87..4af7d0c22 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -207,3 +207,40 @@ end end @testset "Measure Demo" begin end + +@testset "SpringDamper" begin + # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo + @named spring_damper = SpringDamper(; + s_relx0 = 0, + d_y = 1, + s_rely0 = 0, + d_phi = 0, + c_y = 5, + c_x = 5, + d_x = 1, + c_phi = 0) + @named body = Body(; j = 0.1, m = 0.5, rx = 1, ry = 1) + @named fixed = Fixed() + @named fixed_translation = FixedTranslation(; rx = -1, ry = 0) + + connections = [ + connect(fixed.frame, fixed_translation.frame_a), + connect(fixed_translation.frame_b, spring_damper.frame_a), + connect(spring_damper.frame_b, body.frame), + ] + @named model = ODESystem(connections, + t, + [], + [], + systems = [ + spring_damper, + body, + fixed, + fixed_translation, + ]) + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) +end \ No newline at end of file From 6c0a910d218566ac7157332d227b61733cab3428 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 30 Nov 2023 02:35:52 +0200 Subject: [PATCH 54/61] Docstring for `SpringDamper` --- src/Mechanical/PlanarMechanics/components.jl | 31 +++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index d20ec8cf8..672a6a21e 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -107,6 +107,7 @@ end """ FixedTranslation(; name, r::AbstractArray, l) + A fixed translation between two components (rigid rod) # Parameters: @@ -116,8 +117,8 @@ A fixed translation between two components (rigid rod) # Connectors: - - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque """ @mtkmodel FixedTranslation begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -152,8 +153,30 @@ A fixed translation between two components (rigid rod) end """ + SpringDamper(; name, c_x = 1, c_y = 1, c_phi = 1e5, d_x = 1, d_y = 1, d_phi = 1, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10) + +Linear 2D translational spring damper model + +# Parameters: + + - `c_x`: [N/m] Spring constant in x dir + - `c_y`: [N/m] Spring constant in y dir + - `c_phi`: [N.m/rad] Spring constant in phi dir + - `d_x`: [N.s/m] Damping constant in x dir + - `d_y`: [N.s/m] Damping constant in y dir + - `d_phi`: [N.m.s/rad] Damping constant in phi dir + - `s_relx0`: [m] Unstretched spring length + - `s_rely0`: [m] Unstretched spring length + - `phi_rel0`: [rad] Unstretched spring angle + - `s_small`: [m] Prevent zero-division if distance between frame_a and frame_b is zero + + +# Connectors: + + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/SpringDamper.mo#L154C20-L154C20 +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/SpringDamper.mo """ @mtkmodel SpringDamper begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -167,7 +190,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 d_phi = 1, [description = "Damping constant in phi dir"] s_relx0 = 0, [description = "Unstretched spring length"] s_rely0 = 0, [description = "Unstretched spring length"] - phi_rel0 = 0, [description = "Unstretched spring length"] + phi_rel0 = 0, [description = "Unstretched spring angle"] s_small = 1.e-10, [ description = "Prevent zero-division if distance between frame_a and frame_b is zero", From 4dd5d2035ae1d8b34e951126103af14c6771b5d2 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Thu, 30 Nov 2023 14:13:28 +0200 Subject: [PATCH 55/61] Add `Spring`, `Damper` components and their test --- .../PlanarMechanics/PlanarMechanics.jl | 2 +- src/Mechanical/PlanarMechanics/components.jl | 134 ++++++++++++++++++ src/Mechanical/PlanarMechanics/joints.jl | 24 ++-- test/Mechanical/planar_mechanics.jl | 38 ++++- 4 files changed, 183 insertions(+), 15 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index a2ee09318..ad8d20648 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -16,7 +16,7 @@ D = Differential(t) export Frame, FrameResolve, PartialTwoFrames, ZeroPosition include("utils.jl") -export Fixed, Body, FixedTranslation, SpringDamper +export Fixed, Body, FixedTranslation, Spring, Damper, SpringDamper include("components.jl") export Revolute, Prismatic diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 672a6a21e..a21eed641 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -152,6 +152,140 @@ A fixed translation between two components (rigid rod) end end +""" + Spring(; name, c_x = 1, c_y = 1, c_phi = 1e5, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10) + +Linear 2D translational spring + +# Parameters: + + - `c_x`: [N/m] Spring constant in x dir + - `c_y`: [N/m] Spring constant in y dir + - `c_phi`: [N.m/rad] Spring constant in phi dir + - `s_relx0`: [m] Unstretched spring length + - `s_rely0`: [m] Unstretched spring length + - `phi_rel0`: [rad] Unstretched spring angle + - `s_small`: [m] Prevent zero-division if distance between frame_a and frame_b is zero + + +# Connectors: + + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Spring.mo +""" +@mtkmodel Spring begin + @extend frame_a, frame_b = partial_frames = PartialTwoFrames() + + @parameters begin + c_x = 1, [description = "Spring constant in x dir"] + c_y = 1, [description = "Spring constant in y dir"] + c_phi = 1.0e5, [description = "Spring constant"] + s_relx0 = 0, [description = "Unstretched spring length"] + s_rely0 = 0, [description = "Unstretched spring length"] + phi_rel0 = 0, [description = "Unstretched spring angle"] + s_small = 1.e-10, + [ + description = "Prevent zero-division if distance between frame_a and frame_b is zero", + ] + end + + @variables begin + s_relx(t) = 0 + s_rely(t) = 0 + phi_rel(t) = 0 + f_x(t) + f_y(t) + end + + begin + r_rel_0 = [s_relx, s_rely, 0] + l = sqrt(r_rel_0' * r_rel_0) + e_rel_0 = r_rel_0 / max(l, s_small) + end + + @equations begin + phi_rel ~ frame_b.phi - frame_a.phi + frame_a.j ~ 0 + frame_b.j ~ 0 + s_relx ~ frame_b.x - frame_a.x + s_rely ~ frame_b.y - frame_a.y + f_x ~ c_x * (s_relx - s_relx0) + f_y ~ c_y * (s_rely - s_rely0) + frame_a.fx ~ -f_x + frame_b.fx ~ f_x + frame_a.fy ~ -f_y + frame_b.fy ~ f_y + end +end + +""" + Damper(; name, d = 1, s_smal = 1.e-10) + +Linear (velocity dependent) damper + +# Parameters: + + - `d`: [N.s/m] Damoing constant + - `s_small`: [m] Prevent zero-division if distance between frame_a and frame_b is zero + + +# Connectors: + + - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Damper.mo +""" +@mtkmodel Damper begin + @extend frame_a, frame_b = partial_frames = PartialTwoFrames() + + @parameters begin + d = 1, [description = "damping constant"] + s_small = 1.e-10, + [ + description = "Prevent zero-division if distance between frame_a and frame_b is zero", + ] + end + + @variables begin + r0x(t) = 0 + r0y(t) = 0 + d0x(t) = 0 + d0y(t) = 0 + vx(t) = 0 + vy(t) = 0 + v(t) + f(t) + end + + begin + r0 = [r0x, r0y] + l = sqrt(r0' * r0) + end + + @equations begin + frame_a.x + r0x ~ frame_b.x + frame_a.y + r0y ~ frame_b.y + D(frame_a.x) + vx ~ D(frame_b.x) + D(frame_a.y) + vy ~ D(frame_b.y) + v ~ [vx, vy]' * [d0x, d0y] + f ~ -d * v + d0x ~ ifelse(l < s_small, r0[1], r0[1] / l) + d0y ~ ifelse(l < s_small, r0[2], r0[2] / l) + frame_a.fx ~ d0x * f + frame_a.fy ~ d0y * f + frame_a.j ~ 0 + frame_a.fx + frame_b.fx ~ 0 + frame_a.fy + frame_b.fy ~ 0 + frame_a.j + frame_b.j ~ 0 + + # lossPower ~ -f * v + end +end + """ SpringDamper(; name, c_x = 1, c_y = 1, c_phi = 1e5, d_x = 1, d_y = 1, d_phi = 1, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10) diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index a34a34066..13422d581 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -22,11 +22,11 @@ A revolute joint """ @component function Revolute(; - name, - constant_phi = nothing, - constant_ω = nothing, - constat_tau = nothing, - use_flange = false) + name, + constant_phi = nothing, + constant_ω = nothing, + constat_tau = nothing, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = Rotational.Fixed() @@ -98,12 +98,12 @@ A prismatic joint - `support` [Support](@ref) if `use_flange == true` """ @component function Prismatic(; - name, - x, - y, - constant_f = 0, - constant_s = 0, - use_flange = false) + name, + x, + y, + constant_f = 0, + constant_s = 0, + use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames @named fixed = TranslationalModelica.Support() @@ -129,7 +129,7 @@ A prismatic joint r0 = e0 * s eqs = [ - ifelse(constant_s === nothing, s ~ s, s ~ constant_s), + # ifelse(constant_s === nothing, s ~ s, s ~ constant_s), ifelse(constant_f === nothing, f ~ f, f ~ constant_f), v ~ D(s), a ~ D(v), diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 4af7d0c22..47a70795c 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -1,6 +1,5 @@ using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics -# using Plots @parameters t D = Differential(t) @@ -243,4 +242,39 @@ end prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) -end \ No newline at end of file +end + +@testset "Spring and damper demo" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo + @named body = Body(; m = 0.5, j = 0.1) + @named fixed = Fixed() + @named spring = Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) + @named damper = Damper(d = 1) + @named prismatic = Prismatic(; x = 0, y = 1) + + connections = [ + connect(fixed.frame, spring.frame_a), + connect(spring.frame_b, body.frame), + connect(damper.frame_a, spring.frame_a), + connect(damper.frame_b, spring.frame_b), + connect(spring.frame_a, prismatic.frame_a), + connect(prismatic.frame_b, spring.frame_b), + ] + + @named model = ODESystem(connections, + t, + [], + [], + systems = [ + body, + fixed, + spring, + damper, + prismatic, + ]) + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) +end From 3094ba3057c42cb34dfb60cb625ce550bd2125d8 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Tue, 19 Dec 2023 08:32:06 +0200 Subject: [PATCH 56/61] Add `Measure Demo` It's broken but just saving it unit I figure out what is going on with the `FixedTranslation` component --- test/Mechanical/planar_mechanics.jl | 58 ++++++++++++++++++++++++++++- 1 file changed, 57 insertions(+), 1 deletion(-) diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 47a70795c..b2e4e2211 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -7,6 +7,7 @@ tspan = (0.0, 3.0) g = -9.807 @testset "Free body" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/FreeBody.mo m = 2 j = 1 @named body = Body(; m, j) @@ -30,6 +31,7 @@ g = -9.807 end @testset "Pendulum" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo @named ceiling = Fixed() @named rod = FixedTranslation(rx = 1.0, ry = 0.0) @named body = Body(m = 1, j = 0.1) @@ -205,7 +207,61 @@ end @test sol[rel_a_sensor2.rel_a_y.u][end] == 0 end -@testset "Measure Demo" begin end +@testset "Measure Demo" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo + @named body = Body(; m = 1, j = 0.1) + @named fixed_translation = FixedTranslation(; rx = 1, ry = 0) + @named fixed = Fixed() + @named body1 = Body(; m = 0.4, j = 0.02) + @named fixed_translation1 = FixedTranslation(; rx = 0.4, ry = 0) + @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame = :world) + @named rel_pos_sensor = RelativePosition(; resolve_in_frame = :world) + @named revolute1 = Revolute() + @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame = :frame_a) + @named rel_v_sensor = RelativeVelocity(; resolve_in_frame = :frame_b) + @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame = :world) + @named rel_a_sensor = RelativeAcceleration(; resolve_in_frame = :frame_b) + @named revolute2 = Revolute() + + connections = [ + connect(fixed_translation.frame_b, body.frame), + connect(fixed_translation1.frame_b, body1.frame), + connect(fixed.frame, revolute1.frame_a), + connect(revolute1.frame_b, fixed_translation.frame_a), + # connect(abs_a_sensor.frame_resolve, abs_a_sensor.frame_a), + connect(revolute2.frame_b, fixed_translation1.frame_a), + connect(revolute2.frame_a, fixed_translation.frame_b), + # connect_sensor(fixed_translation.frame_b, rel_a_sensor.frame_a)..., + # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), + # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), + # connect(rel_a_sensor.frame_b, body1.frame_a), + # connect(rel_v_sensor.frame_b, body1.frame_a), + # connect(rel_v_sensor.frame_b, body1.frame_a), + connect_sensor(body1.frame, abs_a_sensor.frame_a)..., + # connect_sensor(body1.frame, abs_v_sensor.frame_a)..., + # connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., + ] + + @named model = ODESystem(connections, + t, + [], + [], + systems = [ + fixed_translation, + body, + fixed, + body1, + fixed_translation1, + revolute1, + revolute2, + abs_pos_sensor, + ]) + sys = structural_simplify(model) + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) + sol = solve(prob, Rodas5P()) + @test_broken SciMLBase.successful_retcode(sol) +end @testset "SpringDamper" begin # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo From 7051dbea205126115cd56e21850c30eb0ee17c83 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Wed, 20 Dec 2023 07:51:19 +0200 Subject: [PATCH 57/61] a bit of tidying --- src/Mechanical/PlanarMechanics/components.jl | 14 ++++++--- src/Mechanical/PlanarMechanics/joints.jl | 3 ++ test/Mechanical/planar_mechanics.jl | 33 ++++++++++++-------- 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index a21eed641..38b8ab729 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -12,6 +12,8 @@ Frame fixed in the planar world frame at a given position and orientation # Connectors: - `frame: 2-dim. Coordinate system + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo """ @mtkmodel Fixed begin @parameters begin @@ -58,12 +60,14 @@ Body component with mass and inertia # Connectors: - `frame`: 2-dim. Coordinate system + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ -@component function Body(; name, m, j, rx = 0, ry = 0, gy = -9.807) +@component function Body(; name, m, I, rx = 0, ry = 0, gy = -9.807) @named frame = Frame() pars = @parameters begin m = m - j = j + I = I gy = gy end @@ -98,7 +102,7 @@ Body component with mass and inertia fy ~ frame.fy, ax ~ fx / m, ay ~ ifelse(gy !== nothing, fy / m + gy, fy / m), - j * α ~ frame.j, + I * α ~ frame.j, ] return compose(ODESystem(eqs, t, vars, pars; name = name), @@ -119,6 +123,8 @@ A fixed translation between two components (rigid rod) - `frame_a` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque - `frame_b` [Frame](@ref) Coordinate system fixed to the component with one cut-force and cut-torque + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/FixedTranslation.mo """ @mtkmodel FixedTranslation begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -148,7 +154,7 @@ A fixed translation between two components (rigid rod) # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 frame_a.fy + frame_b.fy ~ 0 - frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0 + frame_a.j + frame_b.j + r0' * [frame_b.fy, -frame_b.fx] ~ 0 end end diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 13422d581..13c33b494 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -20,6 +20,7 @@ A revolute joint - `flange_a` [Flange](@ref) if `use_flange == true` - `support` [Support](@ref) if `use_flange == true` +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Joints/Revolute.mo """ @component function Revolute(; name, @@ -96,6 +97,8 @@ A prismatic joint - `fixed` [Fixed](@ref) if `use_flange == false` - `flange_a` [Flange](@ref) if `use_flange == true` - `support` [Support](@ref) if `use_flange == true` + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Joints/Prismatic.mo """ @component function Prismatic(; name, diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index b2e4e2211..0c8bee52b 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -1,3 +1,5 @@ +# using Revise +# using Plots using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics @@ -9,8 +11,8 @@ g = -9.807 @testset "Free body" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/FreeBody.mo m = 2 - j = 1 - @named body = Body(; m, j) + I = 1 + @named body = Body(; m, I) @named model = ODESystem(Equation[], t, [], @@ -34,7 +36,7 @@ end # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo @named ceiling = Fixed() @named rod = FixedTranslation(rx = 1.0, ry = 0.0) - @named body = Body(m = 1, j = 0.1) + @named body = Body(m = 1, I = 0.1) @named revolute = Revolute() connections = [ @@ -50,7 +52,12 @@ end systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) - @test length(states(sys)) == 7 + @test_broken length(states(sys)) == 2 + unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) + + sol = solve(prob, Rodas5P()) + @test_broken SciMLBase.successful_retcode(sol) end @testset "Prismatic" begin @@ -61,12 +68,12 @@ end @testset "AbsoluteAccCentrifugal" begin # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanicsTest/Sensors.mo#L221-L332 m = 1 - j = 0.1 + I = 0.1 ω = 10 resolve_in_frame = :world # components - @named body = Body(; m, j, gy = 0.0) + @named body = Body(; m, I, gy = 0.0) @named fixed_translation = FixedTranslation(; rx = 10.0, ry = 0.0) @named fixed = Fixed() @named revolute = Revolute(constant_ω = ω) @@ -114,11 +121,11 @@ end @testset "Sensors (two free falling bodies)" begin m = 1 - j = 1 + I = 1 resolve_in_frame = :world - @named body1 = Body(; m, j) - @named body2 = Body(; m, j) + @named body1 = Body(; m, I) + @named body2 = Body(; m, I) @named base = Fixed() @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) @@ -209,10 +216,10 @@ end @testset "Measure Demo" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo - @named body = Body(; m = 1, j = 0.1) + @named body = Body(; m = 1, I = 0.1) @named fixed_translation = FixedTranslation(; rx = 1, ry = 0) @named fixed = Fixed() - @named body1 = Body(; m = 0.4, j = 0.02) + @named body1 = Body(; m = 0.4, I = 0.02) @named fixed_translation1 = FixedTranslation(; rx = 0.4, ry = 0) @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame = :world) @named rel_pos_sensor = RelativePosition(; resolve_in_frame = :world) @@ -274,7 +281,7 @@ end c_x = 5, d_x = 1, c_phi = 0) - @named body = Body(; j = 0.1, m = 0.5, rx = 1, ry = 1) + @named body = Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) @named fixed = Fixed() @named fixed_translation = FixedTranslation(; rx = -1, ry = 0) @@ -302,7 +309,7 @@ end @testset "Spring and damper demo" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo - @named body = Body(; m = 0.5, j = 0.1) + @named body = Body(; m = 0.5, I = 0.1) @named fixed = Fixed() @named spring = Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) @named damper = Damper(d = 1) From 9382fa3aa5780f48ae00b58835c00580a7269c9a Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 8 Mar 2024 13:17:03 +0200 Subject: [PATCH 58/61] Focus on the pendulum model work --- src/Mechanical/PlanarMechanics/joints.jl | 20 +- test/Mechanical/planar_mechanics.jl | 552 +++++++++++------------ 2 files changed, 283 insertions(+), 289 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index 13c33b494..dba125e6d 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -24,14 +24,10 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 """ @component function Revolute(; name, - constant_phi = nothing, - constant_ω = nothing, - constat_tau = nothing, use_flange = false) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames - @named fixed = Rotational.Fixed() - systems = [frame_a, frame_b, fixed] + systems = [frame_a, frame_b] vars = @variables begin phi(t) = 0.0 @@ -41,9 +37,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 end eqs = [ - phi ~ ifelse(constant_phi === nothing, phi, constant_phi), - ω ~ ifelse(constant_ω === nothing, D(phi), constant_ω), - α ~ ifelse(constant_ω === nothing, D(ω), 0.0), + ω ~ D(phi), + α ~ D(ω), # rigidly connect positions frame_a.x ~ frame_b.x, frame_a.y ~ frame_b.y, @@ -53,11 +48,12 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 frame_a.fy + frame_b.fy ~ 0, # balance torques frame_a.j + frame_b.j ~ 0, - j ~ ifelse(constat_tau === nothing, j, constat_tau), - frame_a.j ~ j, + frame_a.j ~ j ] if use_flange + @named fixed = Rotational.Fixed() + push!(systems, fixed) @named flange_a = Rotational.Flange(; phi, tau) push!(systems, flange_a) @named support = Rotational.Support() @@ -127,7 +123,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 end R = [cos(frame_a.phi) -sin(frame_a.phi); - sin(frame_a.phi) cos(frame_a.phi)] + sin(frame_a.phi) cos(frame_a.phi)] e0 = R * [x, y] r0 = e0 * s @@ -143,7 +139,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 frame_a.fx + frame_b.fx ~ 0, frame_a.fy + frame_b.fy ~ 0, frame_a.j + frame_b.j + r0[1] * frame_b.fy - r0[2] * frame_b.fx ~ 0, - e0[1] * frame_a.fx + e0[2] * frame_a.fy ~ f, + e0[1] * frame_a.fx + e0[2] * frame_a.fy ~ f ] if use_flange diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 0c8bee52b..2d99eede5 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -42,7 +42,7 @@ end connections = [ connect(ceiling.frame, revolute.frame_a), connect(revolute.frame_b, rod.frame_a), - connect(rod.frame_b, body.frame), + connect(rod.frame_b, body.frame) ] @named model = ODESystem(connections, @@ -65,279 +65,277 @@ end @test_nowarn @named prismatic = Prismatic(x = 1.0, y = 0.0) end -@testset "AbsoluteAccCentrifugal" begin - # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanicsTest/Sensors.mo#L221-L332 - m = 1 - I = 0.1 - ω = 10 - resolve_in_frame = :world - - # components - @named body = Body(; m, I, gy = 0.0) - @named fixed_translation = FixedTranslation(; rx = 10.0, ry = 0.0) - @named fixed = Fixed() - @named revolute = Revolute(constant_ω = ω) - - # sensors - @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) - - eqs = [ - connect(fixed.frame, revolute.frame_a), - connect(revolute.frame_b, fixed_translation.frame_a), - connect(fixed_translation.frame_b, body.frame), - connect_sensor(body.frame, abs_v_sensor.frame_a)..., - ] - - @named model = ODESystem(eqs, - t, - [], - [], - systems = [ - body, - fixed_translation, - fixed, - revolute, - abs_v_sensor, - ]) - sys = structural_simplify(model) - u0 = [0.0, ω, 0.0] - prob = ODEProblem(sys, u0, tspan, []; jac = true) - sol = solve(prob, Rodas5P()) - - # phi - @test sol[body.phi][end] ≈ tspan[end] * ω - @test all(sol[body.ω] .≈ ω) - - test_points = [i / ω for i in 0:0.1:10] - - # instantaneous linear velocity - v_singal(t) = -ω^2 * sin.(ω .* t) - @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) - - # instantaneous linear acceleration - a_singal(t) = -ω^3 * cos.(ω .* t) - @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) -end - -@testset "Sensors (two free falling bodies)" begin - m = 1 - I = 1 - resolve_in_frame = :world - - @named body1 = Body(; m, I) - @named body2 = Body(; m, I) - @named base = Fixed() - - @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) - @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) - @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame) - @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) - @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) - @named rel_v_sensor1 = RelativeVelocity(; resolve_in_frame) - @named rel_v_sensor2 = RelativeVelocity(; resolve_in_frame) - @named rel_a_sensor1 = RelativeAcceleration(; resolve_in_frame) - @named rel_a_sensor2 = RelativeAcceleration(; resolve_in_frame) - - connections = [ - connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., - connect_sensor(body1.frame, abs_v_sensor.frame_a)..., - connect_sensor(body1.frame, abs_a_sensor.frame_a)..., - connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., - connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., - connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., - connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., - connect_sensor(base.frame, rel_v_sensor1.frame_a)..., - connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., - connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., - connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., - connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., - connect_sensor(base.frame, rel_a_sensor1.frame_b)..., - connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., - connect_sensor(body2.frame, rel_a_sensor2.frame_b)..., - ] - - @named model = ODESystem(connections, - t, - [], - [], - systems = [ - body1, - body2, - base, - abs_pos_sensor, - abs_v_sensor, - abs_a_sensor, - rel_pos_sensor1, - rel_pos_sensor2, - rel_v_sensor1, - rel_v_sensor2, - rel_a_sensor1, - rel_a_sensor2, - ]) - - sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) - - sol = solve(prob, Rodas5P()) - @test SciMLBase.successful_retcode(sol) - - # the two bodyies falled the same distance, and so the absolute sensor attached to body1 - @test sol[abs_pos_sensor.y.u][end] ≈ sol[body1.ry][end] ≈ sol[body2.ry][end] ≈ - 0.5 * g * tspan[end]^2 - - # sensor1 is attached to body1, so the relative y-position between body1 and the base is - # equal to the absolute y-position of body1 - @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.rel_y.u][end] - - # the relative y-position between body1 and body2 is zero - @test sol[rel_pos_sensor2.rel_y.u][end] == 0 - - # no displacement in the x-direction - @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] - - # velocity after t seconds v = g * t, so the relative y-velocity between body1 and the base is - # equal to the absolute y-velocity of body1 - @test sol[abs_v_sensor.v_y.u][end] ≈ sol[rel_v_sensor1.rel_v_y.u][end] ≈ g * tspan[end] - - # the relative y-velocity between body1 and body2 is zero - @test sol[rel_v_sensor2.rel_v_y.u][end] == 0 - - # the body is under constant acclertation = g - @test all(sol[abs_a_sensor.a_y.u] .≈ g) - - # the relative y-accleration between body1 and the base is - # equal to the absolute y-accleration of body1 - @test sol[abs_a_sensor.a_y.u][end] ≈ -sol[rel_a_sensor1.rel_a_y.u][end] - - # the relative y-accleration between body1 and body2 is zero - @test sol[rel_a_sensor2.rel_a_y.u][end] == 0 -end - -@testset "Measure Demo" begin - # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo - @named body = Body(; m = 1, I = 0.1) - @named fixed_translation = FixedTranslation(; rx = 1, ry = 0) - @named fixed = Fixed() - @named body1 = Body(; m = 0.4, I = 0.02) - @named fixed_translation1 = FixedTranslation(; rx = 0.4, ry = 0) - @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame = :world) - @named rel_pos_sensor = RelativePosition(; resolve_in_frame = :world) - @named revolute1 = Revolute() - @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame = :frame_a) - @named rel_v_sensor = RelativeVelocity(; resolve_in_frame = :frame_b) - @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame = :world) - @named rel_a_sensor = RelativeAcceleration(; resolve_in_frame = :frame_b) - @named revolute2 = Revolute() - - connections = [ - connect(fixed_translation.frame_b, body.frame), - connect(fixed_translation1.frame_b, body1.frame), - connect(fixed.frame, revolute1.frame_a), - connect(revolute1.frame_b, fixed_translation.frame_a), - # connect(abs_a_sensor.frame_resolve, abs_a_sensor.frame_a), - connect(revolute2.frame_b, fixed_translation1.frame_a), - connect(revolute2.frame_a, fixed_translation.frame_b), - # connect_sensor(fixed_translation.frame_b, rel_a_sensor.frame_a)..., - # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), - # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), - # connect(rel_a_sensor.frame_b, body1.frame_a), - # connect(rel_v_sensor.frame_b, body1.frame_a), - # connect(rel_v_sensor.frame_b, body1.frame_a), - connect_sensor(body1.frame, abs_a_sensor.frame_a)..., - # connect_sensor(body1.frame, abs_v_sensor.frame_a)..., - # connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., - ] - - @named model = ODESystem(connections, - t, - [], - [], - systems = [ - fixed_translation, - body, - fixed, - body1, - fixed_translation1, - revolute1, - revolute2, - abs_pos_sensor, - ]) - sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) - sol = solve(prob, Rodas5P()) - @test_broken SciMLBase.successful_retcode(sol) -end - -@testset "SpringDamper" begin - # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo - @named spring_damper = SpringDamper(; - s_relx0 = 0, - d_y = 1, - s_rely0 = 0, - d_phi = 0, - c_y = 5, - c_x = 5, - d_x = 1, - c_phi = 0) - @named body = Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) - @named fixed = Fixed() - @named fixed_translation = FixedTranslation(; rx = -1, ry = 0) - - connections = [ - connect(fixed.frame, fixed_translation.frame_a), - connect(fixed_translation.frame_b, spring_damper.frame_a), - connect(spring_damper.frame_b, body.frame), - ] - @named model = ODESystem(connections, - t, - [], - [], - systems = [ - spring_damper, - body, - fixed, - fixed_translation, - ]) - sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) - sol = solve(prob, Rodas5P()) - @test SciMLBase.successful_retcode(sol) -end - -@testset "Spring and damper demo" begin - # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo - @named body = Body(; m = 0.5, I = 0.1) - @named fixed = Fixed() - @named spring = Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) - @named damper = Damper(d = 1) - @named prismatic = Prismatic(; x = 0, y = 1) - - connections = [ - connect(fixed.frame, spring.frame_a), - connect(spring.frame_b, body.frame), - connect(damper.frame_a, spring.frame_a), - connect(damper.frame_b, spring.frame_b), - connect(spring.frame_a, prismatic.frame_a), - connect(prismatic.frame_b, spring.frame_b), - ] - - @named model = ODESystem(connections, - t, - [], - [], - systems = [ - body, - fixed, - spring, - damper, - prismatic, - ]) - sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) - sol = solve(prob, Rodas5P()) - @test SciMLBase.successful_retcode(sol) -end +# @testset "AbsoluteAccCentrifugal" begin +# # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanicsTest/Sensors.mo#L221-L332 +# m = 1 +# I = 0.1 +# ω = 10 +# resolve_in_frame = :world + +# # components +# @named body = Body(; m, I, gy = 0.0) +# @named fixed_translation = FixedTranslation(; rx = 10.0, ry = 0.0) +# @named fixed = Fixed() +# @named revolute = Revolute(constant_ω = ω) + +# # sensors +# @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) + +# eqs = [ +# connect(fixed.frame, revolute.frame_a), +# connect(revolute.frame_b, fixed_translation.frame_a), +# connect(fixed_translation.frame_b, body.frame), +# connect_sensor(body.frame, abs_v_sensor.frame_a)... +# ] + +# @named model = ODESystem(eqs, +# t, +# [], +# [], +# systems = [ +# body, +# fixed_translation, +# fixed, +# revolute, +# abs_v_sensor +# ]) +# sys = structural_simplify(model) +# u0 = [0.0, ω, 0.0] +# prob = ODEProblem(sys, u0, tspan, []; jac = true) +# sol = solve(prob, Rodas5P()) + +# # phi +# @test sol[body.phi][end] ≈ tspan[end] * ω +# @test all(sol[body.ω] .≈ ω) + +# test_points = [i / ω for i in 0:0.1:10] + +# # instantaneous linear velocity +# v_singal(t) = -ω^2 * sin.(ω .* t) +# @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) + +# # instantaneous linear acceleration +# a_singal(t) = -ω^3 * cos.(ω .* t) +# @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) +# end + +# @testset "Sensors (two free falling bodies)" begin +# m = 1 +# I = 1 +# resolve_in_frame = :world + +# @named body1 = Body(; m, I) +# @named body2 = Body(; m, I) +# @named base = Fixed() + +# @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame) +# @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame) +# @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame) +# @named rel_pos_sensor1 = RelativePosition(; resolve_in_frame) +# @named rel_pos_sensor2 = RelativePosition(; resolve_in_frame) +# @named rel_v_sensor1 = RelativeVelocity(; resolve_in_frame) +# @named rel_v_sensor2 = RelativeVelocity(; resolve_in_frame) +# @named rel_a_sensor1 = RelativeAcceleration(; resolve_in_frame) +# @named rel_a_sensor2 = RelativeAcceleration(; resolve_in_frame) + +# connections = [ +# connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., +# connect_sensor(body1.frame, abs_v_sensor.frame_a)..., +# connect_sensor(body1.frame, abs_a_sensor.frame_a)..., +# connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., +# connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., +# connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., +# connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., +# connect_sensor(base.frame, rel_v_sensor1.frame_a)..., +# connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., +# connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., +# connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., +# connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., +# connect_sensor(base.frame, rel_a_sensor1.frame_b)..., +# connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., +# connect_sensor(body2.frame, rel_a_sensor2.frame_b)... +# ] + +# @named model = ODESystem(connections, +# t, +# [], +# [], +# systems = [ +# body1, +# body2, +# base, +# abs_pos_sensor, +# abs_v_sensor, +# abs_a_sensor, +# rel_pos_sensor1, +# rel_pos_sensor2, +# rel_v_sensor1, +# rel_v_sensor2, +# rel_a_sensor1, +# rel_a_sensor2 +# ]) + +# sys = structural_simplify(model) +# unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) +# prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) + +# sol = solve(prob, Rodas5P()) +# @test SciMLBase.successful_retcode(sol) + +# # the two bodyies falled the same distance, and so the absolute sensor attached to body1 +# @test sol[abs_pos_sensor.y.u][end] ≈ sol[body1.ry][end] ≈ sol[body2.ry][end] ≈ +# 0.5 * g * tspan[end]^2 + +# # sensor1 is attached to body1, so the relative y-position between body1 and the base is +# # equal to the absolute y-position of body1 +# @test sol[body1.ry][end] ≈ -sol[rel_pos_sensor1.rel_y.u][end] + +# # the relative y-position between body1 and body2 is zero +# @test sol[rel_pos_sensor2.rel_y.u][end] == 0 + +# # no displacement in the x-direction +# @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.rx][end] ≈ sol[body2.rx][end] + +# # velocity after t seconds v = g * t, so the relative y-velocity between body1 and the base is +# # equal to the absolute y-velocity of body1 +# @test sol[abs_v_sensor.v_y.u][end] ≈ sol[rel_v_sensor1.rel_v_y.u][end] ≈ g * tspan[end] + +# # the relative y-velocity between body1 and body2 is zero +# @test sol[rel_v_sensor2.rel_v_y.u][end] == 0 + +# # the body is under constant acclertation = g +# @test all(sol[abs_a_sensor.a_y.u] .≈ g) + +# # the relative y-accleration between body1 and the base is +# # equal to the absolute y-accleration of body1 +# @test sol[abs_a_sensor.a_y.u][end] ≈ -sol[rel_a_sensor1.rel_a_y.u][end] + +# # the relative y-accleration between body1 and body2 is zero +# @test sol[rel_a_sensor2.rel_a_y.u][end] == 0 +# end + +# @testset "Measure Demo" begin +# # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo +# @named body = Body(; m = 1, I = 0.1) +# @named fixed_translation = FixedTranslation(; rx = 1, ry = 0) +# @named fixed = Fixed() +# @named body1 = Body(; m = 0.4, I = 0.02) +# @named fixed_translation1 = FixedTranslation(; rx = 0.4, ry = 0) +# @named abs_pos_sensor = AbsolutePosition(; resolve_in_frame = :world) +# @named rel_pos_sensor = RelativePosition(; resolve_in_frame = :world) +# @named revolute1 = Revolute() +# @named abs_v_sensor = AbsoluteVelocity(; resolve_in_frame = :frame_a) +# @named rel_v_sensor = RelativeVelocity(; resolve_in_frame = :frame_b) +# @named abs_a_sensor = AbsoluteAcceleration(; resolve_in_frame = :world) +# @named rel_a_sensor = RelativeAcceleration(; resolve_in_frame = :frame_b) +# @named revolute2 = Revolute() + +# connections = [ +# connect(fixed_translation.frame_b, body.frame), +# connect(fixed_translation1.frame_b, body1.frame), +# connect(fixed.frame, revolute1.frame_a), +# connect(revolute1.frame_b, fixed_translation.frame_a), +# # connect(abs_a_sensor.frame_resolve, abs_a_sensor.frame_a), +# connect(revolute2.frame_b, fixed_translation1.frame_a), +# connect(revolute2.frame_a, fixed_translation.frame_b), +# # connect_sensor(fixed_translation.frame_b, rel_a_sensor.frame_a)..., +# # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), +# # connect(fixed_translation.frame_b, rel_v_sensor.frame_a), +# # connect(rel_a_sensor.frame_b, body1.frame_a), +# # connect(rel_v_sensor.frame_b, body1.frame_a), +# # connect(rel_v_sensor.frame_b, body1.frame_a), +# connect_sensor(body1.frame, abs_a_sensor.frame_a)... # connect_sensor(body1.frame, abs_v_sensor.frame_a)..., # connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., +# ] + +# @named model = ODESystem(connections, +# t, +# [], +# [], +# systems = [ +# fixed_translation, +# body, +# fixed, +# body1, +# fixed_translation1, +# revolute1, +# revolute2, +# abs_pos_sensor +# ]) +# sys = structural_simplify(model) +# unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) +# prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) +# sol = solve(prob, Rodas5P()) +# @test_broken SciMLBase.successful_retcode(sol) +# end + +# @testset "SpringDamper" begin +# # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo +# @named spring_damper = SpringDamper(; +# s_relx0 = 0, +# d_y = 1, +# s_rely0 = 0, +# d_phi = 0, +# c_y = 5, +# c_x = 5, +# d_x = 1, +# c_phi = 0) +# @named body = Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) +# @named fixed = Fixed() +# @named fixed_translation = FixedTranslation(; rx = -1, ry = 0) + +# connections = [ +# connect(fixed.frame, fixed_translation.frame_a), +# connect(fixed_translation.frame_b, spring_damper.frame_a), +# connect(spring_damper.frame_b, body.frame) +# ] +# @named model = ODESystem(connections, +# t, +# [], +# [], +# systems = [ +# spring_damper, +# body, +# fixed, +# fixed_translation +# ]) +# sys = structural_simplify(model) +# unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) +# prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) +# sol = solve(prob, Rodas5P()) +# @test SciMLBase.successful_retcode(sol) +# end + +# @testset "Spring and damper demo" begin +# # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo +# @named body = Body(; m = 0.5, I = 0.1) +# @named fixed = Fixed() +# @named spring = Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) +# @named damper = Damper(d = 1) +# @named prismatic = Prismatic(; x = 0, y = 1) + +# connections = [ +# connect(fixed.frame, spring.frame_a), +# connect(spring.frame_b, body.frame), +# connect(damper.frame_a, spring.frame_a), +# connect(damper.frame_b, spring.frame_b), +# connect(spring.frame_a, prismatic.frame_a), +# connect(prismatic.frame_b, spring.frame_b) +# ] + +# @named model = ODESystem(connections, +# t, +# [], +# [], +# systems = [ +# body, +# fixed, +# spring, +# damper, +# prismatic +# ]) +# sys = structural_simplify(model) +# unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) +# prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []; jac = true) +# sol = solve(prob, Rodas5P()) +# @test SciMLBase.successful_retcode(sol) +# end From 883821288d4c1ff0b0b38d5079ca525568498fae Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 8 Mar 2024 14:12:20 +0200 Subject: [PATCH 59/61] Update to support mtk@9 --- src/Mechanical/PlanarMechanics/PlanarMechanics.jl | 8 +++----- test/Mechanical/planar_mechanics.jl | 9 ++++----- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl index ad8d20648..41bc328f9 100644 --- a/src/Mechanical/PlanarMechanics/PlanarMechanics.jl +++ b/src/Mechanical/PlanarMechanics/PlanarMechanics.jl @@ -6,13 +6,11 @@ module PlanarMechanics import ModelingToolkitStandardLibrary.Mechanical.Rotational import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica +using ModelingToolkit: t_nounits as t, D_nounits as D using ModelingToolkit, Symbolics, IfElse using ...Blocks: RealInput, RealOutput import ...@symcheck -@parameters t -D = Differential(t) - export Frame, FrameResolve, PartialTwoFrames, ZeroPosition include("utils.jl") @@ -23,7 +21,7 @@ export Revolute, Prismatic include("joints.jl") export AbsolutePosition, - RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAcceleration, - RelativeAcceleration, connect_sensor + RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAcceleration, + RelativeAcceleration, connect_sensor include("sensors.jl") end diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 2d99eede5..7509732db 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -1,10 +1,9 @@ # using Revise # using Plots using ModelingToolkit, OrdinaryDiffEq, Test +using ModelingToolkit: t_nounits as t, D_nounits as D using ModelingToolkitStandardLibrary.Mechanical.PlanarMechanics -@parameters t -D = Differential(t) tspan = (0.0, 3.0) g = -9.807 @@ -19,7 +18,7 @@ g = -9.807 [], systems = [body]) sys = structural_simplify(model) - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) @@ -52,8 +51,8 @@ end systems = [body, revolute, rod, ceiling]) sys = structural_simplify(model) - @test_broken length(states(sys)) == 2 - unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys))) + @test_broken length(unknowns(sys)) == 2 + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) prob = ODEProblem(sys, unset_vars .=> 0.0, tspan, []; jac = true) sol = solve(prob, Rodas5P()) From 86b99559ba19753400f6d7733a0c17b36ec9012e Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 8 Mar 2024 15:28:33 +0200 Subject: [PATCH 60/61] fmt --- src/Mechanical/PlanarMechanics/components.jl | 14 +- src/Mechanical/PlanarMechanics/sensors.jl | 136 +++++++++---------- 2 files changed, 75 insertions(+), 75 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 38b8ab729..6b39fc307 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -102,7 +102,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 fy ~ frame.fy, ax ~ fx / m, ay ~ ifelse(gy !== nothing, fy / m + gy, fy / m), - I * α ~ frame.j, + I * α ~ frame.j ] return compose(ODESystem(eqs, t, vars, pars; name = name), @@ -132,17 +132,17 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 @parameters begin rx = 0, [ - description = "Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0", + description = "Fixed x-length of the rod resolved w.r.t to body frame_a at phi = 0" ] ry = 0, [ - description = "Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0", + description = "Fixed y-length of the rod resolved w.r.t to body frame_a at phi = 0" ] end begin R = [cos(frame_a.phi) -sin(frame_a.phi); - sin(frame_a.phi) cos(frame_a.phi)] + sin(frame_a.phi) cos(frame_a.phi)] r0 = R * [rx, ry] end @@ -193,7 +193,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 phi_rel0 = 0, [description = "Unstretched spring angle"] s_small = 1.e-10, [ - description = "Prevent zero-division if distance between frame_a and frame_b is zero", + description = "Prevent zero-division if distance between frame_a and frame_b is zero" ] end @@ -252,7 +252,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 d = 1, [description = "damping constant"] s_small = 1.e-10, [ - description = "Prevent zero-division if distance between frame_a and frame_b is zero", + description = "Prevent zero-division if distance between frame_a and frame_b is zero" ] end @@ -333,7 +333,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 phi_rel0 = 0, [description = "Unstretched spring angle"] s_small = 1.e-10, [ - description = "Prevent zero-division if distance between frame_a and frame_b is zero", + description = "Prevent zero-division if distance between frame_a and frame_b is zero" ] end diff --git a/src/Mechanical/PlanarMechanics/sensors.jl b/src/Mechanical/PlanarMechanics/sensors.jl index e190d7a06..cff39b7f6 100644 --- a/src/Mechanical/PlanarMechanics/sensors.jl +++ b/src/Mechanical/PlanarMechanics/sensors.jl @@ -120,14 +120,14 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but frame_a.phi] elseif resolve_in_frame == :frame_a rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0; - sin(frame_a.phi) cos(frame_a.phi) 0; - 0 0 1] + sin(frame_a.phi) cos(frame_a.phi) 0; + 0 0 1] r = transpose(rotation_matrix) * [frame_a.x, frame_a.y, frame_a.phi] - [0, 0, frame_a.phi] elseif resolve_in_frame == :frame_resolve rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0; - sin(frame_resolve.phi) cos(frame_resolve.phi) 0; - 0 0 1] + sin(frame_resolve.phi) cos(frame_resolve.phi) 0; + 0 0 1] r = transpose(rotation_matrix) * [frame_a.x, frame_a.y, frame_a.phi] - [0, 0, frame_resolve.phi] else @@ -140,7 +140,7 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but phi.u ~ r[3], frame_a.fx ~ 0, frame_a.fy ~ 0, - frame_a.j ~ 0, + frame_a.j ~ 0 ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -176,7 +176,7 @@ Measure absolute position and orientation of the origin of frame connector x.u ~ pos.x.u, y.u ~ pos.y.u, phi.u ~ pos.phi.u, - connect(pos.frame_a, frame_a), + connect(pos.frame_a, frame_a) ] if resolve_in_frame == :frame_resolve @@ -221,22 +221,22 @@ Measure relative position and orientation between the origins of two frame conne if resolve_in_frame == :frame_a rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0; - sin(frame_a.phi) cos(frame_a.phi) 0; - 0 0 1] + sin(frame_a.phi) cos(frame_a.phi) 0; + 0 0 1] r = transpose(rotation_matrix) * [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] elseif resolve_in_frame == :frame_b rotation_matrix = [cos(frame_b.phi) -sin(frame_b.phi) 0; - sin(frame_b.phi) cos(frame_b.phi) 0; - 0 0 1] + sin(frame_b.phi) cos(frame_b.phi) 0; + 0 0 1] r = transpose(rotation_matrix) * [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] elseif resolve_in_frame == :world r = [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] elseif resolve_in_frame == :frame_resolve rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0; - sin(frame_resolve.phi) cos(frame_resolve.phi) 0; - 0 0 1] + sin(frame_resolve.phi) cos(frame_resolve.phi) 0; + 0 0 1] r = transpose(rotation_matrix) * [frame_b.x - frame_a.x, frame_b.y - frame_a.y, frame_b.phi - frame_a.phi] else @@ -252,7 +252,7 @@ Measure relative position and orientation between the origins of two frame conne frame_a.j ~ 0, frame_b.fx ~ 0, frame_b.fy ~ 0, - frame_b.j ~ 0, + frame_b.j ~ 0 ] return compose(ODESystem(eqs, t, [], []; name = name), @@ -289,7 +289,7 @@ Measure relative position and orientation between the origins of two frame conne pos.rel_y.u ~ rel_y.u, pos.rel_phi.u ~ rel_phi.u, connect(pos.frame_a, frame_a), - connect(pos.frame_b, frame_b), + connect(pos.frame_b, frame_b) ] if resolve_in_frame == :frame_resolve @@ -309,9 +309,9 @@ Measure relative position and orientation between the origins of two frame conne end @component function BasicTransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -333,7 +333,7 @@ end frame_a.j ~ 0, frame_resolve.fx ~ 0, frame_resolve.fy ~ 0, - frame_resolve.j ~ 0, + frame_resolve.j ~ 0 ] r_temp = Vector{Float64}(undef, 3) @@ -343,21 +343,21 @@ end append!(eqs, [ x_out.u ~ x_in.u, y_out.u ~ y_in.u, - phi_out.u ~ phi_in.u, + phi_out.u ~ phi_in.u ]) else if frame_in == :world R1 = [1.0 0.0 0.0 0.0; - 0.0 1.0 0.0 0.0; - 0.0 0.0 1.0 0.0] + 0.0 1.0 0.0 0.0; + 0.0 0.0 1.0 0.0] elseif frame_in == :frame_a R1 = [cos(frame_a.phi) -sin(frame_a.phi) 0.0 0.0; - sin(frame_a.phi) cos(frame_a.phi) 0.0 0.0; - 0.0 0.0 1.0 frame_a.phi] + sin(frame_a.phi) cos(frame_a.phi) 0.0 0.0; + 0.0 0.0 1.0 frame_a.phi] elseif frame_in == :frame_resolve R1 = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0 0.0; - sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0 0.0; - 0.0 0.0 1.0 frame_resolve.phi] + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0 0.0; + 0.0 0.0 1.0 frame_resolve.phi] else error("Wrong value for parameter frame_in") end @@ -368,27 +368,27 @@ end append!(eqs, [ x_out.u ~ r_temp[1], y_out.u ~ r_temp[2], - phi_out.u ~ r_temp[3], + phi_out.u ~ r_temp[3] ]) elseif frame_out == :frame_a rotation_matrix = [cos(frame_a.phi) sin(frame_a.phi) 0.0; - -sin(frame_a.phi) cos(frame_a.phi) 0.0; - 0.0 0.0 1.0] + -sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] r = rotation_matrix * r_temp append!(eqs, [ x_out.u ~ r[1], y_out.u ~ r[2], - phi_out.u ~ r[3], + phi_out.u ~ r[3] ]) elseif frame_out == :frame_resolve rotation_matrix = [cos(frame_resolve.phi) sin(frame_resolve.phi) 0.0; - -sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; - 0.0 0.0 1.0] + -sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] r = rotation_matrix * r_temp append!(eqs, [ x_out.u ~ r[1], y_out.u ~ r[2], - phi_out.u ~ r[3], + phi_out.u ~ r[3] ]) else error("Wrong value for parameter frame_out") @@ -399,9 +399,9 @@ end end @component function TransformAbsoluteVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named frame_a = Frame() @named x_in = RealInput() @@ -424,7 +424,7 @@ end # in connect(basic_transform_vector.x_in, x_in), connect(basic_transform_vector.y_in, y_in), - connect(basic_transform_vector.phi_in, phi_in), + connect(basic_transform_vector.phi_in, phi_in) ] if frame_in == :frame_resolve || frame_out == :frame_resolve @@ -472,7 +472,7 @@ end transform_absolute_vector.phi_out.u ~ ω.u, connect(pos.frame_a, frame_a), connect(zero_pos.frame_resolve, pos.frame_resolve), - connect(transform_absolute_vector.frame_a, frame_a), + connect(transform_absolute_vector.frame_a, frame_a) ] if resolve_in_frame == :frame_resolve @@ -493,9 +493,9 @@ end end @component function BasicTransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -516,7 +516,7 @@ end phi_out, frame_a, frame_b, - frame_resolve, + frame_resolve ] eqs = Equation[] @@ -527,25 +527,25 @@ end append!(eqs, [ x_out.u ~ x_in.u, y_out.u ~ y_in.u, - phi_out.u ~ phi_in.u, + phi_out.u ~ phi_in.u ]) else if frame_in == :world R1 = [1.0 0.0 0.0; - 0.0 1.0 0.0; - 0.0 0.0 1.0] + 0.0 1.0 0.0; + 0.0 0.0 1.0] elseif frame_in == :frame_a R1 = [cos(frame_a.phi) -sin(frame_a.phi) 0.0; - sin(frame_a.phi) cos(frame_a.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] elseif frame_in == :frame_b R1 = [cos(frame_b.phi) -sin(frame_b.phi) 0.0; - sin(frame_b.phi) cos(frame_b.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_b.phi) cos(frame_b.phi) 0.0; + 0.0 0.0 1.0] else R1 = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0; - sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] end r_in = [x_in.u, y_in.u, phi_in.u] @@ -553,24 +553,24 @@ end r_out = R1 * r_in elseif frame_out == :frame_a rotation_matrix = [cos(frame_a.phi) -sin(frame_a.phi) 0.0; - sin(frame_a.phi) cos(frame_a.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_a.phi) cos(frame_a.phi) 0.0; + 0.0 0.0 1.0] r_out = transpose(rotation_matrix) * (R1 * r_in) elseif frame_out == :frame_b rotation_matrix = [cos(frame_b.phi) -sin(frame_b.phi) 0.0; - sin(frame_b.phi) cos(frame_b.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_b.phi) cos(frame_b.phi) 0.0; + 0.0 0.0 1.0] r_out = transpose(rotation_matrix) * (R1 * r_in) else rotation_matrix = [cos(frame_resolve.phi) -sin(frame_resolve.phi) 0.0; - sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; - 0.0 0.0 1.0] + sin(frame_resolve.phi) cos(frame_resolve.phi) 0.0; + 0.0 0.0 1.0] r_out = transpose(rotation_matrix) * (R1 * r_in) end append!(eqs, [ x_out.u ~ r_out[1], y_out.u ~ r_out[2], - phi_out.u ~ r_out[3], + phi_out.u ~ r_out[3] ]) end @@ -578,9 +578,9 @@ end end @component function TransformRelativeVector(; - name, - frame_in = :frame_a, - frame_out = frame_in) + name, + frame_in = :frame_a, + frame_out = frame_in) @named x_in = RealInput() @named y_in = RealInput() @named phi_in = RealInput() @@ -602,7 +602,7 @@ end phi_out, basic_transformb_vector, frame_a, - frame_b, + frame_b ] eqs = [ @@ -615,7 +615,7 @@ end # in connect(basic_transformb_vector.x_in, x_in), connect(basic_transformb_vector.y_in, y_in), - connect(basic_transformb_vector.phi_in, phi_in), + connect(basic_transformb_vector.phi_in, phi_in) ] if frame_in == :frame_resolve || frame_out == :frame_resolve @@ -652,7 +652,7 @@ end rel_v_y, rel_ω, rel_pos, - transform_relative_vector, + transform_relative_vector ] eqs = [ # connect(relativePosition.r_rel, der_r_rel.u) @@ -667,7 +667,7 @@ end connect(rel_pos.frame_a, frame_a), connect(rel_pos.frame_b, frame_b), connect(transform_relative_vector.frame_a, frame_a), - connect(transform_relative_vector.frame_b, frame_b), + connect(transform_relative_vector.frame_b, frame_b) ] if resolve_in_frame == :frame_resolve @@ -717,7 +717,7 @@ end transform_absolute_vector.phi_out.u ~ α.u, connect(pos.frame_a, frame_a), connect(zero_pos.frame_resolve, pos.frame_resolve), - connect(transform_absolute_vector.frame_a, frame_a), + connect(transform_absolute_vector.frame_a, frame_a) ] if resolve_in_frame == :frame_resolve @@ -757,7 +757,7 @@ end rel_a_y, rel_α, rel_pos, - transform_relative_vector, + transform_relative_vector ] eqs = [ @@ -774,7 +774,7 @@ end connect(rel_pos.frame_a, frame_a), connect(rel_pos.frame_b, frame_b), connect(transform_relative_vector.frame_a, frame_a), - connect(transform_relative_vector.frame_b, frame_b), + connect(transform_relative_vector.frame_b, frame_b) ] if resolve_in_frame == :frame_resolve @@ -799,6 +799,6 @@ function connect_sensor(component_frame, sensor_frame) return [ component_frame.x ~ sensor_frame.x, component_frame.y ~ sensor_frame.y, - component_frame.phi ~ sensor_frame.phi, + component_frame.phi ~ sensor_frame.phi ] end \ No newline at end of file From 611a12490472687a2f0d164de61414398616c715 Mon Sep 17 00:00:00 2001 From: Abdulrhmn Ghanem Date: Fri, 8 Mar 2024 15:35:15 +0200 Subject: [PATCH 61/61] typos --- src/Mechanical/PlanarMechanics/components.jl | 2 +- src/Mechanical/PlanarMechanics/joints.jl | 4 ++-- test/Mechanical/planar_mechanics.jl | 14 +++++++------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/Mechanical/PlanarMechanics/components.jl b/src/Mechanical/PlanarMechanics/components.jl index 6b39fc307..fdf448980 100644 --- a/src/Mechanical/PlanarMechanics/components.jl +++ b/src/Mechanical/PlanarMechanics/components.jl @@ -227,7 +227,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 end """ - Damper(; name, d = 1, s_smal = 1.e-10) + Damper(; name, d = 1, s_small = 1.e-10) Linear (velocity dependent) damper diff --git a/src/Mechanical/PlanarMechanics/joints.jl b/src/Mechanical/PlanarMechanics/joints.jl index dba125e6d..2989c8aa6 100644 --- a/src/Mechanical/PlanarMechanics/joints.jl +++ b/src/Mechanical/PlanarMechanics/joints.jl @@ -4,8 +4,8 @@ A revolute joint # parameters - `use_flange=false`: If `true`, a force flange is enabled, otherwise implicitly grounded" - - `phi`: [rad] Intial angular position for the flange - - `tau`: [N.m] Inital Cut torque in the flange + - `phi`: [rad] Initial angular position for the flange + - `tau`: [N.m] Initial Cut torque in the flange # states - `phi(t)`: [rad] angular position diff --git a/test/Mechanical/planar_mechanics.jl b/test/Mechanical/planar_mechanics.jl index 7509732db..115635768 100644 --- a/test/Mechanical/planar_mechanics.jl +++ b/test/Mechanical/planar_mechanics.jl @@ -110,12 +110,12 @@ end # test_points = [i / ω for i in 0:0.1:10] # # instantaneous linear velocity -# v_singal(t) = -ω^2 * sin.(ω .* t) -# @test all(v_singal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) +# v_signal(t) = -ω^2 * sin.(ω .* t) +# @test all(v_signal.(test_points) .≈ sol.(test_points; idxs = abs_v_sensor.v_x.u)) # # instantaneous linear acceleration -# a_singal(t) = -ω^3 * cos.(ω .* t) -# @test all(a_singal.(test_points) .≈ sol.(test_points; idxs = body.ax)) +# a_signal(t) = -ω^3 * cos.(ω .* t) +# @test all(a_signal.(test_points) .≈ sol.(test_points; idxs = body.ax)) # end # @testset "Sensors (two free falling bodies)" begin @@ -205,11 +205,11 @@ end # # the body is under constant acclertation = g # @test all(sol[abs_a_sensor.a_y.u] .≈ g) -# # the relative y-accleration between body1 and the base is -# # equal to the absolute y-accleration of body1 +# # the relative y-acceleration between body1 and the base is +# # equal to the absolute y-acceleration of body1 # @test sol[abs_a_sensor.a_y.u][end] ≈ -sol[rel_a_sensor1.rel_a_y.u][end] -# # the relative y-accleration between body1 and body2 is zero +# # the relative y-acceleration between body1 and body2 is zero # @test sol[rel_a_sensor2.rel_a_y.u][end] == 0 # end