From c312d754ec6ecefb3d57f307eabd24f2d02f0e87 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 08:34:18 +0200 Subject: [PATCH 1/6] add PlanarMechanics module originally implemented in https://github.com/SciML/ModelingToolkitStandardLibrary.jl/pull/220 --- src/Multibody.jl | 2 + src/PlanarMechanics/PlanarMechanics.jl | 27 + src/PlanarMechanics/components.jl | 378 ++++++++++++ src/PlanarMechanics/joints.jl | 156 +++++ src/PlanarMechanics/sensors.jl | 804 +++++++++++++++++++++++++ src/PlanarMechanics/utils.jl | 60 ++ test/runtests.jl | 5 + test/test_PlanarMechanics.jl | 364 +++++++++++ 8 files changed, 1796 insertions(+) create mode 100644 src/PlanarMechanics/PlanarMechanics.jl create mode 100644 src/PlanarMechanics/components.jl create mode 100644 src/PlanarMechanics/joints.jl create mode 100644 src/PlanarMechanics/sensors.jl create mode 100644 src/PlanarMechanics/utils.jl create mode 100644 test/test_PlanarMechanics.jl diff --git a/src/Multibody.jl b/src/Multibody.jl index eb5adbe7..dab02f3e 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -210,5 +210,7 @@ include("robot/robot_components.jl") include("robot/FullRobot.jl") +export PlanarMechanics +include("PlanarMechanics/PlanarMechanics.jl") end diff --git a/src/PlanarMechanics/PlanarMechanics.jl b/src/PlanarMechanics/PlanarMechanics.jl new file mode 100644 index 00000000..5751903b --- /dev/null +++ b/src/PlanarMechanics/PlanarMechanics.jl @@ -0,0 +1,27 @@ +""" +Library to model planar mechanical multi-body systems inspired by https://github.com/dzimmer/PlanarMechanics +""" + +module PlanarMechanics + +import ModelingToolkitStandardLibrary.Mechanical.Rotational +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica +using ModelingToolkit: t_nounits as t, D_nounits as D +using ModelingToolkit +using ...Blocks: RealInput, RealOutput +import ...@symcheck + +export Frame, FrameResolve, PartialTwoFrames, ZeroPosition +include("utils.jl") + +export Fixed, Body, FixedTranslation, Spring, Damper, SpringDamper +include("components.jl") + +export Revolute, Prismatic +include("joints.jl") + +export AbsolutePosition, + RelativePosition, AbsoluteVelocity, RelativeVelocity, AbsoluteAcceleration, + RelativeAcceleration, connect_sensor +include("sensors.jl") +end diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl new file mode 100644 index 00000000..33dbbf5b --- /dev/null +++ b/src/PlanarMechanics/components.jl @@ -0,0 +1,378 @@ +""" + Fixed(; name, r = (0.0, 0.0), phi = 0.0) + +Frame fixed in the planar world frame at a given position and orientation + +# Parameters: + + - `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: + + - `frame: 2-dim. Coordinate system + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo +""" +@mtkmodel Fixed begin + @parameters begin + 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() + end + + @equations begin + frame.x ~ x + frame.y ~ y + 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 defaults to -9.807 + +# 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 + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo +""" +@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807) + @named frame = Frame() + pars = @parameters begin + m = m + I = I + gy = gy + end + + vars = @variables begin + fx(t) + fy(t) + rx(t) = rx + ry(t) = ry + vx(t) + vy(t) + ax(t) + ay(t) + phi(t) = phi + ω(t) + α(t) + end + + eqs = [ + # velocity is the time derivative of position + rx ~ frame.x, + ry ~ frame.y, + vx ~ D(rx), + vy ~ D(ry), + phi ~ frame.phi, + ω ~ D(phi), + # acceleration is the time derivative of velocity + ax ~ D(vx), + ay ~ D(vy), + α ~ D(ω), + # newton's law + fx ~ frame.fx, + fy ~ frame.fy, + ax ~ fx / m, + ay ~ fy / m + gy,#ifelse(gy !== nothing, fy / m + gy, fy / m), + I * α ~ frame.j + ] + + return compose(ODESystem(eqs, t; name), + frame) +end + +""" + FixedTranslation(; name, r::AbstractArray, l) + +A fixed translation between two components (rigid rod) + +# Parameters: + + - `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: + + - `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() + + @parameters begin + rx = 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" + ] + end + + 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 + 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 + +""" + 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_small = 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) + +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 +""" +@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 angle"] + 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/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl new file mode 100644 index 00000000..eadeb4c6 --- /dev/null +++ b/src/PlanarMechanics/joints.jl @@ -0,0 +1,156 @@ +""" + 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] Initial angular position for the flange + - `tau`: [N.m] Initial 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` + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Joints/Revolute.mo +""" +@component function Revolute(; + name, + use_flange = false) + @named partial_frames = PartialTwoFrames() + @unpack frame_a, frame_b = partial_frames + systems = [frame_a, frame_b] + + vars = @variables begin + (phi(t) = 0.0), [state_priority=10] + (ω(t) = 0.0), [state_priority=10] + α(t) + j(t) + end + + eqs = [ + ω ~ D(phi), + α ~ D(ω), + # rigidly connect positions + 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 + @named fixed = Rotational.Fixed() + push!(systems, fixed) + @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 + 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 + - `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 + - `f(t)`: [N] Force in direction of elongation + +# 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` + +https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Joints/Prismatic.mo +""" +@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 = constant_f, constant_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 + 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 + r0[1] ~ frame_b.x, + frame_a.y + r0[2] ~ frame_b.y, + frame_a.phi ~ frame_b.phi, + 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 + ] + + if use_flange + push!(eqs, connect(fixed.flange, support)) + else + # actutation torque + push!(eqs, constant_f ~ 0) + end + + pars = [] + + return compose(ODESystem(eqs, t, vars, pars; name = name), + systems...) +end diff --git a/src/PlanarMechanics/sensors.jl b/src/PlanarMechanics/sensors.jl new file mode 100644 index 00000000..cff39b7f --- /dev/null +++ b/src/PlanarMechanics/sensors.jl @@ -0,0 +1,804 @@ +""" + 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 + # TODO: assert the number of connections + # https://github.com/dzimmer/PlanarMechanics/blob/443b007bcc1522bb172f13012e2d7a8ecc3f7a9b/PlanarMechanics/Sensors/Internal/PartialAbsoluteSensor.mo#L11 +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) +# 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 + +""" + 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). + +# Connectors: + + - `x`: [m] x-position + - `y`: [m] y-position + - `phi`: [rad] rotation angle (counterclockwise) + - `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 x, y, phi r is resolved (1: :world, 2: :frame_a, 3: :frame_resolve) +""" +@component function BasicAbsolutePosition(; name, resolve_in_frame = :frame_a) + @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("resolve_in_frame must be one of :world, :frame_a, :frame_resolve") + end + + eqs = [ + 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), + x, y, phi, frame_a, frame_resolve) +end + +""" + AbsolutePosition(;name, resolve_in_frame = :frame_a) +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 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(; resolve_in_frame) + @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 = [ + x.u ~ pos.x.u, + y.u ~ pos.y.u, + phi.u ~ pos.phi.u, + 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 + +""" + BasicRelativePosition(; name, resolve_in_frame = :frame_a) +Measure relative position and orientation between the origins of two frame connectors + +# Connectors: + + - `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`: + +# 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 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 + + 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 = [ + 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), + rel_x, rel_y, rel_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: + + - `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: + + - `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 rel_x = RealOutput() + @named rel_y = RealOutput() + @named rel_phi = RealOutput() + + systems = [pos, frame_a, frame_b, rel_x, rel_y, rel_phi] + eqs = [ + 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) + ] + + 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 + +@component function BasicTransformAbsoluteVector(; + 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 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] + 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 + +@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_transform_vector = BasicTransformAbsoluteVector(; frame_in, frame_out) + + systems = [frame_a, x_in, y_in, phi_in, x_out, y_out, phi_out, basic_transform_vector] + + eqs = [ + connect(basic_transform_vector.frame_a, frame_a), + # 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_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_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_transform_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(transform_absolute_vector.zero_pos.frame_resolve, + zero_pos1.frame_resolve)) + end + + return compose(ODESystem(eqs, t, [], []; name = name), systems...) +end + +@component 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 + +@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 + +@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 + +@component function AbsoluteAcceleration(; name, resolve_in_frame = :frame_a) + @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 + +@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 + +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/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl new file mode 100644 index 00000000..7665e44d --- /dev/null +++ b/src/PlanarMechanics/utils.jl @@ -0,0 +1,60 @@ +@connector Frame begin + x(t), [description = "x position"] + y(t), [description = "y position"] + phi(t), [state_priority=2, 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 + +# 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() + 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 + +""" + 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 diff --git a/test/runtests.jl b/test/runtests.jl index 833f7634..6f497807 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -51,6 +51,11 @@ end include("test_worldforces.jl") end +@testset "PlanarMechanics" begin + @info "Testing PlanarMechanics" + include("test_PlanarMechanics.jl") +end + # ============================================================================== ## Add spring to make a harmonic oscillator ==================================== diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl new file mode 100644 index 00000000..5aff0313 --- /dev/null +++ b/test/test_PlanarMechanics.jl @@ -0,0 +1,364 @@ +# using Revise +# using Plots +using ModelingToolkit, OrdinaryDiffEq, Test +using ModelingToolkit: t_nounits as t, D_nounits as D +import Multibody.PlanarMechanics as Planar +using JuliaSimCompiler + +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 + I = 1 + @named body = Planar.Body(; m, I) + @named model = ODESystem(Equation[], + t, + [], + [], + systems = [body]) + sys = structural_simplify(IRSystem(model)) + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan) + + sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) + @test SciMLBase.successful_retcode(sol) + + 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 + +@testset "Pendulum" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo + @named ceiling = Planar.Fixed() + @named rod = Planar.FixedTranslation(rx = 1.0, ry = 0.0) + @named body = Planar.Body(m = 1, I = 0.1) + @named revolute = Planar.Revolute() + + 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]) + ssys = structural_simplify(IRSystem(model)) + + @test length(unknowns(ssys)) == 2 + unset_vars = setdiff(unknowns(ssys), keys(ModelingToolkit.defaults(ssys))) + prob = ODEProblem(ssys, unset_vars .=> 0.0, tspan) + + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) +end + +@testset "Prismatic" begin + # just testing instantiation + @test_nowarn @named prismatic = Planar.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 = Planar.Body(; m, I, gy = 0.0) + @named fixed_translation = Planar.FixedTranslation(; rx = 10.0, ry = 0.0) + @named fixed = Planar.Fixed() + @named revolute = Planar.Revolute()#constant_ω = ω) + + # sensors + @named abs_v_sensor = Planar.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), + # Planar.connect_sensor(body.frame, abs_v_sensor.frame_a)... # QUESTION: why? + connect(body.frame, abs_v_sensor.frame_a) + ] + + @named model = ODESystem(eqs, + t, + [], + [], + systems = [ + body, + fixed_translation, + fixed, + revolute, + abs_v_sensor + ]) + model = complete(model) + @test_skip begin # Yingbo: BoundsError: attempt to access 137-element Vector{Vector{Int64}} at index [138] + ssys = structural_simplify(IRSystem(model)) + prob = ODEProblem(ssys, [model.body.ω => ω], tspan) + sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) + + # 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_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_signal(t) = -ω^3 * cos.(ω .* t) + @test all(a_signal.(test_points) .≈ sol.(test_points; idxs = body.ax)) + end +end + +@testset "Sensors (two free falling bodies)" begin + m = 1 + I = 1 + resolve_in_frame = :world + + @named body1 = Planar.Body(; m, I) + @named body2 = Planar.Body(; m, I) + @named base = Planar.Fixed() + + @named abs_pos_sensor = Planar.AbsolutePosition(; resolve_in_frame) + @named abs_v_sensor = Planar.AbsoluteVelocity(; resolve_in_frame) + @named abs_a_sensor = Planar.AbsoluteAcceleration(; resolve_in_frame) + @named rel_pos_sensor1 = Planar.RelativePosition(; resolve_in_frame) + @named rel_pos_sensor2 = Planar.RelativePosition(; resolve_in_frame) + @named rel_v_sensor1 = Planar.RelativeVelocity(; resolve_in_frame) + @named rel_v_sensor2 = Planar.RelativeVelocity(; resolve_in_frame) + @named rel_a_sensor1 = Planar.RelativeAcceleration(; resolve_in_frame) + @named rel_a_sensor2 = Planar.RelativeAcceleration(; resolve_in_frame) + + connections = [ + Planar.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., + Planar.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., + Planar.connect_sensor(body1.frame, abs_a_sensor.frame_a)..., + Planar.connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., + Planar.connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., + Planar.connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., + Planar.connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., + Planar.connect_sensor(base.frame, rel_v_sensor1.frame_a)..., + Planar.connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., + Planar.connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., + Planar.connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., + Planar.connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., + Planar.connect_sensor(base.frame, rel_a_sensor1.frame_b)..., + Planar.connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., + Planar.connect_sensor(body2.frame, rel_a_sensor2.frame_b)... + ] + + # connections = [ + # connect(body1.frame, abs_pos_sensor.frame_a), + # connect(body1.frame, abs_v_sensor.frame_a), + # connect(body1.frame, abs_a_sensor.frame_a), + # connect(body1.frame, rel_pos_sensor1.frame_a), + # connect(base.frame, rel_pos_sensor1.frame_b), + # connect(body1.frame, rel_pos_sensor2.frame_a), + # connect(body2.frame, rel_pos_sensor2.frame_b), + # connect(base.frame, rel_v_sensor1.frame_a), + # connect(body1.frame, rel_v_sensor1.frame_b), + # connect(body1.frame, rel_v_sensor2.frame_a), + # connect(body2.frame, rel_v_sensor2.frame_b), + # connect(body1.frame, rel_a_sensor1.frame_a), + # connect(base.frame, rel_a_sensor1.frame_b), + # connect(body1.frame, rel_a_sensor2.frame_a), + # connect(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)) # Yingbo: fails with JSCompiler + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, tspan) + + 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-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-acceleration 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 = Planar.Body(; m = 1, I = 0.1) + @named fixed_translation = Planar.FixedTranslation(; rx = 1, ry = 0) + @named fixed = Planar.Fixed() + @named body1 = Planar.Body(; m = 0.4, I = 0.02) + @named fixed_translation1 = Planar.FixedTranslation(; rx = 0.4, ry = 0) + @named abs_pos_sensor = Planar.AbsolutePosition(; resolve_in_frame = :world) + @named rel_pos_sensor = Planar.RelativePosition(; resolve_in_frame = :world) + @named revolute1 = Planar.Revolute() + @named abs_v_sensor = Planar.AbsoluteVelocity(; resolve_in_frame = :frame_a) + @named rel_v_sensor = Planar.RelativeVelocity(; resolve_in_frame = :frame_b) + @named abs_a_sensor = Planar.AbsoluteAcceleration(; resolve_in_frame = :world) + @named rel_a_sensor = Planar.RelativeAcceleration(; resolve_in_frame = :frame_b) + @named revolute2 = Planar.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), + # Planar.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), + Planar.connect_sensor(body1.frame, abs_a_sensor.frame_a)... # Planar.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., # Planar.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 + ]) + @test_skip begin # Yingbo: BoundsError again + sys = structural_simplify(IRSystem(model)) + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5)) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + end +end + +@testset "SpringDamper" begin + # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo + @named spring_damper = Planar.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 = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) + @named fixed = Planar.Fixed() + @named fixed_translation = Planar.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(IRSystem(model)) + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5)) + 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 = Planar.Body(; m = 0.5, I = 0.1) + @named fixed = Planar.Fixed() + @named spring = Planar.Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) + @named damper = Planar.Damper(d = 1) + @named prismatic = Planar.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)) # Yingbo: fails with JSCompiler + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []) + sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) + @test SciMLBase.successful_retcode(sol) +end From b51aa0043f6590d8cbf3f9f0143060f5626331dc Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 09:01:11 +0200 Subject: [PATCH 2/6] switch to array r in Body and FixedTrans --- src/PlanarMechanics/components.jl | 46 ++++++++++++------------------- test/test_PlanarMechanics.jl | 34 ++++++++++++----------- 2 files changed, 36 insertions(+), 44 deletions(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 33dbbf5b..571616a9 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -72,14 +72,10 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 end vars = @variables begin - fx(t) - fy(t) - rx(t) = rx - ry(t) = ry - vx(t) - vy(t) - ax(t) - ay(t) + f(t)[1:2] + r(t)[1:2] = [rx, ry] + v(t)[1:2] + a(t)[1:2] phi(t) = phi ω(t) α(t) @@ -87,21 +83,16 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 eqs = [ # velocity is the time derivative of position - rx ~ frame.x, - ry ~ frame.y, - vx ~ D(rx), - vy ~ D(ry), - phi ~ frame.phi, - ω ~ D(phi), + r .~ [frame.x, frame.y] + v .~ D.(r) + phi ~ frame.phi + ω .~ D.(phi) # acceleration is the time derivative of velocity - ax ~ D(vx), - ay ~ D(vy), - α ~ D(ω), + a .~ D.(v) + α .~ D.(ω) # newton's law - fx ~ frame.fx, - fy ~ frame.fy, - ax ~ fx / m, - ay ~ fy / m + gy,#ifelse(gy !== nothing, fy / m + gy, fy / m), + f .~ [frame.fx, frame.fy] + f .~ m*a + m*[0, -gy]#ifelse(gy !== nothing, fy / m + gy, fy / m), I * α ~ frame.j ] @@ -130,20 +121,19 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @parameters begin - rx = 0, + r[1:2] = [1.0, 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 x,y-length of the rod resolved w.r.t to body frame_a at phi = 0" ] end + begin + r = collect(r) + end begin R = [cos(frame_a.phi) -sin(frame_a.phi); sin(frame_a.phi) cos(frame_a.phi)] - r0 = R * [rx, ry] + r0 = R * r end @equations begin diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 5aff0313..6d7d734f 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -26,16 +26,16 @@ g = -9.807 @test SciMLBase.successful_retcode(sol) 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 sol[body.r[2]][end] ≈ free_falling_displacement + @test sol[body.r[1]][end] == 0 # no horizontal displacement @test all(sol[body.phi] .== 0) - # plot(sol, idxs = [body.rx, body.ry]) + # plot(sol, idxs = [body.r[1], body.r[2]]) end @testset "Pendulum" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo @named ceiling = Planar.Fixed() - @named rod = Planar.FixedTranslation(rx = 1.0, ry = 0.0) + @named rod = Planar.FixedTranslation(r = [1.0, 0.0]) @named body = Planar.Body(m = 1, I = 0.1) @named revolute = Planar.Revolute() @@ -74,7 +74,7 @@ end # components @named body = Planar.Body(; m, I, gy = 0.0) - @named fixed_translation = Planar.FixedTranslation(; rx = 10.0, ry = 0.0) + @named fixed_translation = Planar.FixedTranslation(; r = [10, 0]) @named fixed = Planar.Fixed() @named revolute = Planar.Revolute()#constant_ω = ω) @@ -204,18 +204,18 @@ 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.r[2]][end] ≈ sol[body2.r[2]][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] + @test sol[body1.r[2]][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] + @test sol[abs_pos_sensor.x.u][end] ≈ sol[body1.r[1]][end] ≈ sol[body2.r[1]][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 @@ -238,10 +238,10 @@ end @testset "Measure Demo" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo @named body = Planar.Body(; m = 1, I = 0.1) - @named fixed_translation = Planar.FixedTranslation(; rx = 1, ry = 0) + @named fixed_translation = Planar.FixedTranslation(;) @named fixed = Planar.Fixed() @named body1 = Planar.Body(; m = 0.4, I = 0.02) - @named fixed_translation1 = Planar.FixedTranslation(; rx = 0.4, ry = 0) + @named fixed_translation1 = Planar.FixedTranslation(; r = [0.4, 0]) @named abs_pos_sensor = Planar.AbsolutePosition(; resolve_in_frame = :world) @named rel_pos_sensor = Planar.RelativePosition(; resolve_in_frame = :world) @named revolute1 = Planar.Revolute() @@ -304,7 +304,7 @@ end c_phi = 0) @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) @named fixed = Planar.Fixed() - @named fixed_translation = Planar.FixedTranslation(; rx = -1, ry = 0) + @named fixed_translation = Planar.FixedTranslation(; r = [-1, 0]) connections = [ connect(fixed.frame, fixed_translation.frame_a), @@ -356,9 +356,11 @@ end damper, prismatic ]) - sys = structural_simplify((model)) # Yingbo: fails with JSCompiler - unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []) - sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) - @test SciMLBase.successful_retcode(sol) + @test_skip begin + sys = structural_simplify(IRSystem(model)) # Yingbo: fails with JSCompiler + unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5), []) + sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) + @test SciMLBase.successful_retcode(sol) + end end From d2742fae1535f26d5651410c386b7604e0294be2 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 09:32:26 +0200 Subject: [PATCH 3/6] add rendering of several planar components --- ext/Render.jl | 100 +++++++++++++++++++++++++ src/PlanarMechanics/PlanarMechanics.jl | 1 + src/PlanarMechanics/components.jl | 19 ++++- test/test_PlanarMechanics.jl | 2 +- 4 files changed, 119 insertions(+), 3 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index b8c76b31..1a8cc41e 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -2,6 +2,7 @@ module Render using Makie using Multibody import Multibody: render, render!, loop_render, encode, decode, get_rot, get_trans, get_frame +import Multibody.PlanarMechanics as P using Rotations using LinearAlgebra using ModelingToolkit @@ -679,4 +680,103 @@ function rot_from_line(d) y = y ./ norm(y) RotMatrix{3}([x y d]) end + +# ============================================================================== +## PlanarMechanics +# ============================================================================== +function get_rot_fun_2d(sol, frame) + phifun = get_fun(sol, frame.phi) + function (t) + phi = phifun(t) + [cos(phi) -sin(phi); sin(phi) cos(phi)] + end +end + +function get_frame_fun_2d(sol, frame) + R = get_rot_fun_2d(sol, frame) + tr = get_fun(sol, [frame.x, frame.y]) + function (t) + [R(t) tr(t); 0 0 1] + end +end + +function render!(scene, ::typeof(P.Body), sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true + color = get_color(sys, sol, :purple) + r_cm = get_fun(sol, collect(sys.r)) + framefun = get_frame_fun_2d(sol, sys.frame) + radius = sol(sol.t[1], idxs=sys.radius) |> Float32 + thing = @lift begin # Sphere + # Ta = framefun($t) + # coords = (Ta*[0; 0; 1])[1:2] # r_cm($t) + + coords = r_cm($t) + point = Point3f(coords..., 0) + Sphere(point, Float32(radius)) + end + mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) + false +end + + +function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t) + r_0a = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) + r_0b = get_fun(sol, [sys.frame_b.x, sys.frame_b.y]) + color = get_color(sys, sol, :purple) + thing = @lift begin + r1 = Point3f(r_0a($t)..., 0) + r2 = Point3f(r_0b($t)..., 0) + origin = r1#(r1+r2) ./ 2 + extremity = r2#-r1 # Double pendulum is a good test for this + radius = 0.1f0#Float32(sol($t, idxs=sys.radius)) + Makie.GeometryBasics.Cylinder(origin, extremity, radius) + end + mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) + true +end + +function render!(scene, ::typeof(P.Revolute), sys, sol, t) + r_0 = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) + n = [0,0,1] + color = get_color(sys, sol, :red) + + rotfun = get_rot_fun_2d(sol, sys.frame_a) + radius = try + sol(sol.t[1], idxs=sys.radius) + catch + 0.05f0 + end |> Float32 + length = try + sol(sol.t[1], idxs=sys.length) + catch + radius + end |> Float32 + thing = @lift begin + O = [r_0($t)..., 0] + R_w_a = cat(rotfun($t), 1, dims=(1,2)) + n_w = R_w_a*n # Rotate to the world frame + p1 = Point3f(O + length*n_w) + p2 = Point3f(O - length*n_w) + Makie.GeometryBasics.Cylinder(p1, p2, radius) + end + mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) + true +end + +function render!(scene, ::Union{typeof(P.Spring), typeof(P.SpringDamper)}, sys, sol, t) + r_0a = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) + r_0b = get_fun(sol, [sys.frame_b.x, sys.frame_b.y]) + color = get_color(sys, sol, :blue) + n_wind = sol(sol.t[1], idxs=sys.num_windings) + radius = sol(sol.t[1], idxs=sys.radius) |> Float32 + N = sol(sol.t[1], idxs=sys.N) |> Int + thing = @lift begin + r1 = Point3f(r_0a($t)..., 0) + r2 = Point3f(r_0b($t)..., 0) + spring_mesh(r1,r2; n_wind, radius, N) + end + plot!(scene, thing; color) + true +end + end \ No newline at end of file diff --git a/src/PlanarMechanics/PlanarMechanics.jl b/src/PlanarMechanics/PlanarMechanics.jl index 5751903b..9f47c596 100644 --- a/src/PlanarMechanics/PlanarMechanics.jl +++ b/src/PlanarMechanics/PlanarMechanics.jl @@ -10,6 +10,7 @@ using ModelingToolkit: t_nounits as t, D_nounits as D using ModelingToolkit using ...Blocks: RealInput, RealOutput import ...@symcheck +import ..Multibody export Frame, FrameResolve, PartialTwoFrames, ZeroPosition include("utils.jl") diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 571616a9..425b1141 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -63,12 +63,15 @@ Body component with mass and inertia https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ -@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807) +@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple) @named frame = Frame() pars = @parameters begin m = m I = I gy = gy + radius = radius, [description = "Radius of the body in animations"] + render = render, [description = "Render the body in animations"] + color[1:4] = color, [description = "Color of the body in animations"] end vars = @variables begin @@ -96,7 +99,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 I * α ~ frame.j ] - return compose(ODESystem(eqs, t; name), + return compose(ODESystem(eqs, t, vars, pars; name), frame) end @@ -125,6 +128,8 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 [ description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0" ] + radius = 0.1, [description = "Radius of the rod in animations"] + render = true, [description = "Render the rod in animations"] end begin r = collect(r) @@ -185,6 +190,11 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 [ description = "Prevent zero-division if distance between frame_a and frame_b is zero" ] + num_windings = 6, [description = "Number of windings of the coil when rendered"] + color[1:4] = [0, 0.0, 1, 1], [description = "Color of the spring in animations"] + render = true, [description = "Render the spring in animations"] + radius = 0.1, [description = "Radius of spring when rendered"] + N = 200, [description = "Number of points in mesh when rendered"] end @variables begin @@ -325,6 +335,11 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 [ description = "Prevent zero-division if distance between frame_a and frame_b is zero" ] + num_windings = 6, [description = "Number of windings of the coil when rendered"] + color[1:4] = [0, 0.0, 1, 1], [description = "Color of the spring in animations"] + render = true, [description = "Render the spring in animations"] + radius = 0.1, [description = "Radius of spring when rendered"] + N = 200, [description = "Number of points in mesh when rendered"] end @variables begin diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 6d7d734f..11d65951 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -302,7 +302,7 @@ end c_x = 5, d_x = 1, c_phi = 0) - @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1) + @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1]) @named fixed = Planar.Fixed() @named fixed_translation = Planar.FixedTranslation(; r = [-1, 0]) From 0223ebf9b061adc64059c9eba34981a198979bc5 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 09:42:03 +0200 Subject: [PATCH 4/6] rename import to avoid conflict --- test/test_PlanarMechanics.jl | 128 +++++++++++++++++------------------ 1 file changed, 64 insertions(+), 64 deletions(-) diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 11d65951..4decd3ff 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -2,7 +2,7 @@ # using Plots using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkit: t_nounits as t, D_nounits as D -import Multibody.PlanarMechanics as Planar +import Multibody.PlanarMechanics as Pl using JuliaSimCompiler tspan = (0.0, 3.0) @@ -12,7 +12,7 @@ g = -9.807 # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/FreeBody.mo m = 2 I = 1 - @named body = Planar.Body(; m, I) + @named body = Pl.Body(; m, I) @named model = ODESystem(Equation[], t, [], @@ -34,10 +34,10 @@ end @testset "Pendulum" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo - @named ceiling = Planar.Fixed() - @named rod = Planar.FixedTranslation(r = [1.0, 0.0]) - @named body = Planar.Body(m = 1, I = 0.1) - @named revolute = Planar.Revolute() + @named ceiling = Pl.Fixed() + @named rod = Pl.FixedTranslation(r = [1.0, 0.0]) + @named body = Pl.Body(m = 1, I = 0.1) + @named revolute = Pl.Revolute() connections = [ connect(ceiling.frame, revolute.frame_a), @@ -62,7 +62,7 @@ end @testset "Prismatic" begin # just testing instantiation - @test_nowarn @named prismatic = Planar.Prismatic(x = 1.0, y = 0.0) + @test_nowarn @named prismatic = Pl.Prismatic(x = 1.0, y = 0.0) end @testset "AbsoluteAccCentrifugal" begin @@ -73,19 +73,19 @@ end resolve_in_frame = :world # components - @named body = Planar.Body(; m, I, gy = 0.0) - @named fixed_translation = Planar.FixedTranslation(; r = [10, 0]) - @named fixed = Planar.Fixed() - @named revolute = Planar.Revolute()#constant_ω = ω) + @named body = Pl.Body(; m, I, gy = 0.0) + @named fixed_translation = Pl.FixedTranslation(; r = [10, 0]) + @named fixed = Pl.Fixed() + @named revolute = Pl.Revolute()#constant_ω = ω) # sensors - @named abs_v_sensor = Planar.AbsoluteVelocity(; resolve_in_frame) + @named abs_v_sensor = Pl.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), - # Planar.connect_sensor(body.frame, abs_v_sensor.frame_a)... # QUESTION: why? + # Pl.connect_sensor(body.frame, abs_v_sensor.frame_a)... # QUESTION: why? connect(body.frame, abs_v_sensor.frame_a) ] @@ -127,36 +127,36 @@ end I = 1 resolve_in_frame = :world - @named body1 = Planar.Body(; m, I) - @named body2 = Planar.Body(; m, I) - @named base = Planar.Fixed() + @named body1 = Pl.Body(; m, I) + @named body2 = Pl.Body(; m, I) + @named base = Pl.Fixed() - @named abs_pos_sensor = Planar.AbsolutePosition(; resolve_in_frame) - @named abs_v_sensor = Planar.AbsoluteVelocity(; resolve_in_frame) - @named abs_a_sensor = Planar.AbsoluteAcceleration(; resolve_in_frame) - @named rel_pos_sensor1 = Planar.RelativePosition(; resolve_in_frame) - @named rel_pos_sensor2 = Planar.RelativePosition(; resolve_in_frame) - @named rel_v_sensor1 = Planar.RelativeVelocity(; resolve_in_frame) - @named rel_v_sensor2 = Planar.RelativeVelocity(; resolve_in_frame) - @named rel_a_sensor1 = Planar.RelativeAcceleration(; resolve_in_frame) - @named rel_a_sensor2 = Planar.RelativeAcceleration(; resolve_in_frame) + @named abs_pos_sensor = Pl.AbsolutePosition(; resolve_in_frame) + @named abs_v_sensor = Pl.AbsoluteVelocity(; resolve_in_frame) + @named abs_a_sensor = Pl.AbsoluteAcceleration(; resolve_in_frame) + @named rel_pos_sensor1 = Pl.RelativePosition(; resolve_in_frame) + @named rel_pos_sensor2 = Pl.RelativePosition(; resolve_in_frame) + @named rel_v_sensor1 = Pl.RelativeVelocity(; resolve_in_frame) + @named rel_v_sensor2 = Pl.RelativeVelocity(; resolve_in_frame) + @named rel_a_sensor1 = Pl.RelativeAcceleration(; resolve_in_frame) + @named rel_a_sensor2 = Pl.RelativeAcceleration(; resolve_in_frame) connections = [ - Planar.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., - Planar.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., - Planar.connect_sensor(body1.frame, abs_a_sensor.frame_a)..., - Planar.connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., - Planar.connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., - Planar.connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., - Planar.connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., - Planar.connect_sensor(base.frame, rel_v_sensor1.frame_a)..., - Planar.connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., - Planar.connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., - Planar.connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., - Planar.connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., - Planar.connect_sensor(base.frame, rel_a_sensor1.frame_b)..., - Planar.connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., - Planar.connect_sensor(body2.frame, rel_a_sensor2.frame_b)... + Pl.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., + Pl.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., + Pl.connect_sensor(body1.frame, abs_a_sensor.frame_a)..., + Pl.connect_sensor(body1.frame, rel_pos_sensor1.frame_a)..., + Pl.connect_sensor(base.frame, rel_pos_sensor1.frame_b)..., + Pl.connect_sensor(body1.frame, rel_pos_sensor2.frame_a)..., + Pl.connect_sensor(body2.frame, rel_pos_sensor2.frame_b)..., + Pl.connect_sensor(base.frame, rel_v_sensor1.frame_a)..., + Pl.connect_sensor(body1.frame, rel_v_sensor1.frame_b)..., + Pl.connect_sensor(body1.frame, rel_v_sensor2.frame_a)..., + Pl.connect_sensor(body2.frame, rel_v_sensor2.frame_b)..., + Pl.connect_sensor(body1.frame, rel_a_sensor1.frame_a)..., + Pl.connect_sensor(base.frame, rel_a_sensor1.frame_b)..., + Pl.connect_sensor(body1.frame, rel_a_sensor2.frame_a)..., + Pl.connect_sensor(body2.frame, rel_a_sensor2.frame_b)... ] # connections = [ @@ -237,19 +237,19 @@ end @testset "Measure Demo" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/MeasureDemo.mo - @named body = Planar.Body(; m = 1, I = 0.1) - @named fixed_translation = Planar.FixedTranslation(;) - @named fixed = Planar.Fixed() - @named body1 = Planar.Body(; m = 0.4, I = 0.02) - @named fixed_translation1 = Planar.FixedTranslation(; r = [0.4, 0]) - @named abs_pos_sensor = Planar.AbsolutePosition(; resolve_in_frame = :world) - @named rel_pos_sensor = Planar.RelativePosition(; resolve_in_frame = :world) - @named revolute1 = Planar.Revolute() - @named abs_v_sensor = Planar.AbsoluteVelocity(; resolve_in_frame = :frame_a) - @named rel_v_sensor = Planar.RelativeVelocity(; resolve_in_frame = :frame_b) - @named abs_a_sensor = Planar.AbsoluteAcceleration(; resolve_in_frame = :world) - @named rel_a_sensor = Planar.RelativeAcceleration(; resolve_in_frame = :frame_b) - @named revolute2 = Planar.Revolute() + @named body = Pl.Body(; m = 1, I = 0.1) + @named fixed_translation = Pl.FixedTranslation(;) + @named fixed = Pl.Fixed() + @named body1 = Pl.Body(; m = 0.4, I = 0.02) + @named fixed_translation1 = Pl.FixedTranslation(; r = [0.4, 0]) + @named abs_pos_sensor = Pl.AbsolutePosition(; resolve_in_frame = :world) + @named rel_pos_sensor = Pl.RelativePosition(; resolve_in_frame = :world) + @named revolute1 = Pl.Revolute() + @named abs_v_sensor = Pl.AbsoluteVelocity(; resolve_in_frame = :frame_a) + @named rel_v_sensor = Pl.RelativeVelocity(; resolve_in_frame = :frame_b) + @named abs_a_sensor = Pl.AbsoluteAcceleration(; resolve_in_frame = :world) + @named rel_a_sensor = Pl.RelativeAcceleration(; resolve_in_frame = :frame_b) + @named revolute2 = Pl.Revolute() connections = [ connect(fixed_translation.frame_b, body.frame), @@ -259,13 +259,13 @@ end # 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), - # Planar.connect_sensor(fixed_translation.frame_b, rel_a_sensor.frame_a)..., + # Pl.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), - Planar.connect_sensor(body1.frame, abs_a_sensor.frame_a)... # Planar.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., # Planar.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., + Pl.connect_sensor(body1.frame, abs_a_sensor.frame_a)... # Pl.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., # Pl.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., ] @named model = ODESystem(connections, @@ -293,7 +293,7 @@ end @testset "SpringDamper" begin # https://github.com/dzimmer/PlanarMechanics/blob/master/PlanarMechanics/Examples/SpringDamperDemo.mo - @named spring_damper = Planar.SpringDamper(; + @named spring_damper = Pl.SpringDamper(; s_relx0 = 0, d_y = 1, s_rely0 = 0, @@ -302,9 +302,9 @@ end c_x = 5, d_x = 1, c_phi = 0) - @named body = Planar.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1]) - @named fixed = Planar.Fixed() - @named fixed_translation = Planar.FixedTranslation(; r = [-1, 0]) + @named body = Pl.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1]) + @named fixed = Pl.Fixed() + @named fixed_translation = Pl.FixedTranslation(; r = [-1, 0]) connections = [ connect(fixed.frame, fixed_translation.frame_a), @@ -330,11 +330,11 @@ end @testset "Spring and damper demo" begin # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/SpringDemo.mo - @named body = Planar.Body(; m = 0.5, I = 0.1) - @named fixed = Planar.Fixed() - @named spring = Planar.Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) - @named damper = Planar.Damper(d = 1) - @named prismatic = Planar.Prismatic(; x = 0, y = 1) + @named body = Pl.Body(; m = 0.5, I = 0.1) + @named fixed = Pl.Fixed() + @named spring = Pl.Spring(; c_y = 10, s_rely0 = -0.5, c_x = 1, c_phi = 1e5) + @named damper = Pl.Damper(d = 1) + @named prismatic = Pl.Prismatic(; x = 0, y = 1) connections = [ connect(fixed.frame, spring.frame_a), From b1feacc3857e2532716db0332b8a970fa7320358 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 09:54:59 +0200 Subject: [PATCH 5/6] switch to array param --- src/PlanarMechanics/components.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 425b1141..6ca37ff7 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -63,7 +63,7 @@ Body component with mass and inertia https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ -@component function Body(; name, m, I, rx = 0, ry = 0, phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple) +@component function Body(; name, m, I, r = zeros(2), phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple) @named frame = Frame() pars = @parameters begin m = m @@ -76,7 +76,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 vars = @variables begin f(t)[1:2] - r(t)[1:2] = [rx, ry] + r(t)[1:2] = r v(t)[1:2] a(t)[1:2] phi(t) = phi From 4c312f7df8437319ca42dcae95f3106511fbd2e6 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 10 Sep 2024 10:31:01 +0200 Subject: [PATCH 6/6] add BodyShape component --- ext/Render.jl | 7 +- src/PlanarMechanics/components.jl | 167 ++++++++++++++++++------------ src/PlanarMechanics/joints.jl | 9 +- test/test_PlanarMechanics.jl | 34 +++++- 4 files changed, 142 insertions(+), 75 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index 1a8cc41e..85f44d4a 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -719,7 +719,8 @@ function render!(scene, ::typeof(P.Body), sys, sol, t) end -function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t) +function render!(scene, ::Union{typeof(P.FixedTranslation), typeof(P.BodyShape)}, sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true r_0a = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) r_0b = get_fun(sol, [sys.frame_b.x, sys.frame_b.y]) color = get_color(sys, sol, :purple) @@ -728,7 +729,7 @@ function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t) r2 = Point3f(r_0b($t)..., 0) origin = r1#(r1+r2) ./ 2 extremity = r2#-r1 # Double pendulum is a good test for this - radius = 0.1f0#Float32(sol($t, idxs=sys.radius)) + radius = Float32(sol($t, idxs=sys.radius)) Makie.GeometryBasics.Cylinder(origin, extremity, radius) end mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) @@ -736,6 +737,7 @@ function render!(scene, ::typeof(P.FixedTranslation), sys, sol, t) end function render!(scene, ::typeof(P.Revolute), sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true r_0 = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) n = [0,0,1] color = get_color(sys, sol, :red) @@ -764,6 +766,7 @@ function render!(scene, ::typeof(P.Revolute), sys, sol, t) end function render!(scene, ::Union{typeof(P.Spring), typeof(P.SpringDamper)}, sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true r_0a = get_fun(sol, [sys.frame_a.x, sys.frame_a.y]) r_0b = get_fun(sol, [sys.frame_b.x, sys.frame_b.y]) color = get_color(sys, sol, :blue) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 6ca37ff7..3540f83d 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -1,3 +1,4 @@ +purple = Multibody.purple """ Fixed(; name, r = (0.0, 0.0), phi = 0.0) @@ -13,7 +14,6 @@ Frame fixed in the planar world frame at a given position and orientation - `frame: 2-dim. Coordinate system -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Fixed.mo """ @mtkmodel Fixed begin @parameters begin @@ -34,44 +34,40 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 end """ - Body(; name, m, j, r, g = nothing) + Body(; name, m=1, I=0.1, r=0, gy=-9.807, radius=0.1, render=true, color=Multibody.purple) 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 defaults to -9.807 - -# 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 +- `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] Translational position x,y-position +- `gy`: [m/s²] gravity field acting on the mass in the y-direction, positive value acts in the positive direction defaults to -9.807 +- `radius`: [m] Radius of the body in animations +- `render`: [Bool] Render the body in animations +- `color`: [Array{Float64,1}] Color of the body in animations + +# Variables: + - `r`: [m, m] x,y position + - `v`: [m/s, m/s] x,y velocity + - `a`: [m/s², m/s²] x,y acceleration - `phi`: [rad] rotation angle (counterclockwise) - `ω`: [rad/s] angular velocity - `α`: [rad/s²] angular acceleration # Connectors: - - `frame`: 2-dim. Coordinate system - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Body.mo """ @component function Body(; name, m, I, r = zeros(2), phi = 0, gy = -9.807, radius=0.1, render=true, color=Multibody.purple) @named frame = Frame() pars = @parameters begin - m = m - I = I - gy = gy + m = m, [description = "Mass of the body"] + I = I, [description = "Inertia of the body with respect to the origin of frame_a along the z-axis of frame_a"] + gy = gy, [description = "Gravity field acting on the mass in the y-direction, positive value acts in the positive direction"] radius = radius, [description = "Radius of the body in animations"] render = render, [description = "Render the body in animations"] color[1:4] = color, [description = "Color of the body in animations"] + z = 0, [description = "Fixed z-position"] end vars = @variables begin @@ -95,7 +91,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 α .~ D.(ω) # newton's law f .~ [frame.fx, frame.fy] - f .~ m*a + m*[0, -gy]#ifelse(gy !== nothing, fy / m + gy, fy / m), + f + [0, m*gy] .~ m*a#ifelse(gy !== nothing, fy / m + gy, fy / m), I * α ~ frame.j ] @@ -103,22 +99,60 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 frame) end +""" + BodyShape(; name, r = [1,0], r_cm = 0.5*r, gy = -9.807) + +The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames and a vector `r` that describes the translation between them, while the body has a single frame only. + +# Parameters +- `r`: (Structural) Vector from `frame_a` to `frame_b` resolved in `frame_a` +- `r_cm`: (Structural) Vector from `frame_a` to the center of mass resolved in `frame_a` +""" +@mtkmodel BodyShape begin + @structural_parameters begin + r = [1,0] + r_cm = 0.5*r + gy = -9.807 + end + @parameters begin + # r[1:2] = [1,0], [description = "Fixed x,y-length of the rod resolved w.r.t to body frame_a at phi = 0"] + # r_cm[1:2] = 0.5*r, [description = "Vector from frame_a to center of mass, resolved in frame_a"] + m = 1, [description = "mass of the body"] + I = 0.1, [description = "inertia of the body with respect to the center of mass"] + radius = 0.1, [description = "Radius of the body in animations"] + render = true, [description = "Render the body in animations"] + (color[1:4] = purple), [description = "Color of the body in animations"] + end + @components begin + translation = FixedTranslation(; r) + translation_cm = FixedTranslation(; r=r_cm) + body = Body(; r=r_cm, I, m, gy) + frame_a = Frame() + frame_b = Frame() + end + @equations begin + connect(frame_a, translation.frame_a, translation_cm.frame_a) + connect(frame_b, translation.frame_b) + connect(translation_cm.frame_b, body.frame) + end +end + """ FixedTranslation(; name, r::AbstractArray, l) A fixed translation between two components (rigid rod) # Parameters: - - - `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 +- `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 +- `radius`: [m] Radius of the rod in animations +- `render`: [Bool] Render the rod in animations # 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 +- `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() @@ -159,22 +193,23 @@ end 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 - +- `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 +- `num_windings`: [Int] Number of windings of the coil when rendered +- `color = [0,0,1,1]` Color of the spring in animations +- `render = true` Render the spring in animations +- `radius = 0.1` Radius of spring when rendered +- `N = 200` Number of points in mesh when rendered # 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 - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/Spring.mo """ @mtkmodel Spring begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -232,18 +267,13 @@ end 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 +- `d`: [N.s/m] Damping 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 +- `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 Damper begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() @@ -298,25 +328,26 @@ end 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 - +- `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 +- `num_windings`: [Int] Number of windings of the coil when rendered +- `color = [0,0,1,1]` Color of the spring in animations +- `render = true` Render the spring in animations +- `radius = 0.1` Radius of spring when rendered +- `N = 200` Number of points in mesh when rendered # 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 - -https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Parts/SpringDamper.mo """ @mtkmodel SpringDamper begin @extend frame_a, frame_b = partial_frames = PartialTwoFrames() diff --git a/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl index eadeb4c6..b1ddd787 100644 --- a/src/PlanarMechanics/joints.jl +++ b/src/PlanarMechanics/joints.jl @@ -24,7 +24,7 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 """ @component function Revolute(; name, - use_flange = false) + use_flange = false, render = true, radius = 0.1, color = [1.0, 0.0, 0.0, 1.0]) @named partial_frames = PartialTwoFrames() @unpack frame_a, frame_b = partial_frames systems = [frame_a, frame_b] @@ -36,6 +36,12 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 j(t) end + pars = @parameters begin + render = render, [description = "Render the joint in animations"] + radius = radius, [description = "Radius of the body in animations"] + color[1:4] = color, [description = "Color of the body in animations"] + end + eqs = [ ω ~ D(phi), α ~ D(ω), @@ -64,7 +70,6 @@ https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b70839146 push!(eqs, j ~ 0) end - pars = [] return compose(ODESystem(eqs, t, vars, pars; name = name), systems...) diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 4decd3ff..8836d9d9 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -47,9 +47,35 @@ end @named model = ODESystem(connections, t, - [], - [], systems = [body, revolute, rod, ceiling]) + model = complete(model) + ssys = structural_simplify(IRSystem(model)) + + @test length(unknowns(ssys)) == 2 + unset_vars = setdiff(unknowns(ssys), keys(ModelingToolkit.defaults(ssys))) + prob = ODEProblem(ssys, unset_vars .=> 0.0, tspan) + + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + @test sol(1, idxs=model.rod.frame_a.phi) ≈ -2.881383661312169 atol=1e-2 + @test sol(2, idxs=model.rod.frame_a.phi) ≈ -1 atol=1e-2 +end + +@testset "Pendulum with body shape" begin + # https://github.com/dzimmer/PlanarMechanics/blob/743462f58858a808202be93b708391461cbe2523/PlanarMechanics/Examples/Pendulum.mo + @named ceiling = Pl.Fixed() + @named rod = Pl.BodyShape(r = [1.0, 0.0], m=1, I=0.1) + @named revolute = Pl.Revolute() + + connections = [ + connect(ceiling.frame, revolute.frame_a), + connect(revolute.frame_b, rod.frame_a), + ] + + @named model = ODESystem(connections, + t, + systems = [revolute, rod, ceiling]) + model = complete(model) ssys = structural_simplify(IRSystem(model)) @test length(unknowns(ssys)) == 2 @@ -58,6 +84,8 @@ end sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) + @test sol(1, idxs=model.rod.frame_a.phi) ≈ -pi atol=1e-2 + @test sol(2, idxs=model.rod.frame_a.phi) ≈ 0 atol=1e-2 end @testset "Prismatic" begin @@ -302,7 +330,7 @@ end c_x = 5, d_x = 1, c_phi = 0) - @named body = Pl.Body(; I = 0.1, m = 0.5, rx = 1, ry = 1, color=[0,1,0,1]) + @named body = Pl.Body(; I = 0.1, m = 0.5, r = [1,1], color=[0,1,0,1]) @named fixed = Pl.Fixed() @named fixed_translation = Pl.FixedTranslation(; r = [-1, 0])