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