LIBRARY
PlanarMechanics.examples.dyad_balans.DyadBalans2DFF
This is a continuation of the DyadBalans2D example, adding a feedforward generator computed using DyadControlSystems.feedforward_generator in the script compute_feedforward.jl. The feedforward generator provides a filtered position reference to the outer controller as well as an angle feedforward reference for the inner controller and a direct torque feedforward signal to be applied directly to the torque input. Both inner feedforward signals are connected to the u_ff port on the LimPID components.
Usage
MultibodyComponents.PlanarMechanics.examples.dyad_balans.DyadBalans2DFF(M=0.1, m=0.05, R=0.04, L=0.08, Ib=2.5e-4, Iw=1e-4)
Parameters:
| Name | Description | Units | Default value |
|---|---|---|---|
M | Body mass | kg | 0.1 |
m | Wheel mass | kg | 0.05 |
R | Wheel radius | m | 0.04 |
L | Distance from wheel axis to body center of mass | m | 0.08 |
Ib | Body moment of inertia | kg.m2 | 0.00025 |
Iw | Wheel + motor moment of inertia | kg.m2 | 0.0001 |
Behavior
julia
using MultibodyComponents #hide
using ModelingToolkit #hide
@variables M #hide
@variables m #hide
@variables R #hide
@variables L #hide
@variables Ib #hide
@variables Iw #hide
@named sys = MultibodyComponents.PlanarMechanics.examples.dyad_balans.DyadBalans2DFF(M=M, m=m, R=R, L=L, Ib=Ib, Iw=Iw) #hide
full_equations(sys) #hide<< @example-block not executed in draft mode >>Source
dyad
"This is a continuation of the DyadBalans2D example, adding a feedforward generator computed using `DyadControlSystems.feedforward_generator` in the script `compute_feedforward.jl`. The feedforward generator provides a filtered position reference to the outer controller as well as an angle feedforward reference for the inner controller and a direct torque feedforward signal to be applied directly to the torque input. Both inner feedforward signals are connected to the `u_ff` port on the `LimPID` components."
example component DyadBalans2DFF
world = MultibodyComponents.PlanarMechanics.World(g = 9.82) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 890, "y1": 900, "x2": 790, "y2": 800, "rot": 180}
},
"tags": []
}
}
wheelJoint = MultibodyComponents.PlanarMechanics.OneDOFRollingWheelJoint(radius = R) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 200, "y1": 700, "x2": 460, "y2": 960, "rot": 0}
},
"tags": []
}
}
IMU = MultibodyComponents.PlanarMechanics.AbsolutePosition(resolve_in_frame = MultibodyComponents.ResolveInFrame.World()) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 460, "y1": 540, "x2": 560, "y2": 640, "rot": 0}
},
"tags": []
}
}
square = BlockComponents.Sources.Square(amplitude = 0.05, frequency = 1 / 10) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -830, "y1": 600, "x2": -730, "y2": 700, "rot": 0}
},
"tags": []
}
}
wheelinertia = MultibodyComponents.PlanarMechanics.Body(I = Iw, radius = 0.01, m = m) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 390, "y1": 680, "x2": 530, "y2": 820, "rot": 0}
},
"tags": []
}
}
simplemotor = MultibodyComponents.PlanarMechanics.examples.dyad_balans.SimpleMotor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 620, "x2": 380, "y2": 720, "rot": 90}
},
"tags": []
}
}
angle_controller = BlockComponents.Continuous.LimPID(k = 0.487401, Ti = 0.0587352, Td = 0.0420526, Nd = 119.368, y_max = 0.1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 30, "y1": 620, "x2": 130, "y2": 720, "rot": 0}
},
"tags": []
}
}
pos_controller = BlockComponents.Continuous.LimPID(k = 0.0666576, Ti = 5.25024, Td = 4.81393, Nd = 4.76616, wd = 1, wp = 1, y_max = deg2rad(25.0)) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -260, "y1": 620, "x2": -160, "y2": 720, "rot": 0}
},
"tags": []
}
}
gain = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 170, "y1": 630, "x2": 250, "y2": 710, "rot": 0}
},
"tags": []
}
}
gain1 = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -120, "y1": 630, "x2": -40, "y2": 710, "rot": 0}
},
"tags": []
}
}
"""
refgen has 1 input (position reference) and 3 outputs:
y[1]: angle feedforward
y[2]: filtered position reference
y[3]: torque feedforward
"""
refgen = BlockComponents.Continuous.StateSpace(nx = ff_nx(), nu = 1, ny = 3, A = ff_A(), B = ff_B(), C = ff_C(), D = ff_D()) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -570, "y1": 600, "x2": -470, "y2": 700, "rot": 0}
},
"tags": []
}
}
angle_ff = MultibodyComponents.Selector(nu = 3) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 470, "x2": -310, "y2": 570, "rot": 0}
},
"tags": []
}
}
pos_ref = MultibodyComponents.Selector(nu = 3, index = 2) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 600, "x2": -310, "y2": 700, "rot": 0}
},
"tags": []
}
}
torque_ff = MultibodyComponents.Selector(nu = 3, index = 3) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 340, "x2": -310, "y2": 440, "rot": 0}
},
"tags": []
}
}
"This negative gain is required since we are providing the angle reference signal to the `u_ff` connector of the outer controller which sits behind the negative gain `gain1`. We are thus effectively negating `gain1` with this `gain2`."
gain2 = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -240, "y1": 540, "x2": -180, "y2": 600, "rot": 90}
},
"tags": []
}
}
body_mass = MultibodyComponents.PlanarMechanics.BodyShape(m = M, I = Ib, radius = 0.03, color = [0.2, 0.2, 0.2, 0.9], r = [0, 2 * L], r_cm = [0, L]) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 130, "y1": 130, "x2": 530, "y2": 530, "rot": 270}
},
"tags": []
}
}
mux1 = MultibodyComponents.PlanarMechanics.examples.dyad_balans.Mux1() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -700, "y1": 600, "x2": -600, "y2": 700, "rot": 0}
},
"tags": []
}
}
"Body mass"
parameter M::Mass = 0.1
"Wheel mass"
parameter m::Mass = 0.05
"Wheel radius"
parameter R::Length = 0.04
"Distance from wheel axis to body center of mass"
parameter L::Length = 0.08
"Body moment of inertia"
parameter Ib::Inertia = 2.5e-4
"Wheel + motor moment of inertia"
parameter Iw::Inertia = 1e-4
relations
initial body_mass.body.phi = 0.1
initial body_mass.body.w = 0.0
initial wheelinertia.phi = 0.0
initial wheelinertia.w = 0.0
connect(wheelinertia.frame_a, wheelJoint.frame_a) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 330, "y": 750}], "E": 2}],
"renderStyle": "standard"
}
}
connect(simplemotor.frame_b, wheelJoint.frame_a) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
y: analysis_point(IMU.phi, angle_controller.u_m)
y2: analysis_point(IMU.x, pos_controller.u_m)
u: analysis_point(angle_controller.y, gain.u)
u2: analysis_point(pos_controller.y, gain1.u)
connect(IMU.phi, angle_controller.u_m) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [
{"x": 660, "y": 621},
{"x": 660, "y": 1040},
{"x": 0, "y": 1040},
{"x": 0, "y": 693}
],
"E": 2
}
],
"renderStyle": "standard"
}
}
connect(IMU.x, pos_controller.u_m) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [
{"x": 710, "y": 560},
{"x": 710, "y": 1090},
{"x": -290, "y": 1090},
{"x": -290, "y": 693}
],
"E": 2
}
],
"renderStyle": "standard"
}
}
connect(pos_controller.y, gain1.u) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -134.25, "y": 671}, {"x": -134.25, "y": 670}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain1.y, angle_controller.u_s) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -10, "y": 670}, {"x": -10, "y": 647}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain.y, simplemotor.torqueinput) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(angle_controller.y, gain.u) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 150.5, "y": 671}, {"x": 150.5, "y": 670}], "E": 2}]
}
}
connect(pos_ref.y, pos_controller.u_s) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -290, "y": 650}, {"x": -290, "y": 647}], "E": 2}],
"renderStyle": "standard"
}
}
connect(refgen.y, pos_ref.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(refgen.y, torque_ff.u, angle_ff.u) {
"Dyad": {
"edges": [
{"S": 1, "M": [{"x": -445, "y": 650}], "E": -1},
{"S": -1, "M": [{"x": -445, "y": 390}], "E": 2},
{"S": 3, "M": [], "E": -1}
],
"junctions": [{"x": -445, "y": 520}],
"renderStyle": "standard"
}
}
connect(body_mass.frame_a, simplemotor.frame_a, IMU.frame_a) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [{"x": 330.00000000000006, "y": 535}, {"x": 330, "y": 535}],
"E": -1
},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [], "E": -1}
],
"junctions": [{"x": 330, "y": 590}],
"renderStyle": "standard"
}
}
connect(square.y, mux1.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(mux1.y, refgen.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(angle_ff.y, gain2.u) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -210, "y": 520}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain2.y, pos_controller.u_ff) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(torque_ff.y, angle_controller.u_ff) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 80, "y": 390}], "E": 2}]
}
}
metadata {"Dyad": {"tests": {"case1": {"stop": 50}}}}
endFlattened Source
dyad
"This is a continuation of the DyadBalans2D example, adding a feedforward generator computed using `DyadControlSystems.feedforward_generator` in the script `compute_feedforward.jl`. The feedforward generator provides a filtered position reference to the outer controller as well as an angle feedforward reference for the inner controller and a direct torque feedforward signal to be applied directly to the torque input. Both inner feedforward signals are connected to the `u_ff` port on the `LimPID` components."
example component DyadBalans2DFF
world = MultibodyComponents.PlanarMechanics.World(g = 9.82) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 890, "y1": 900, "x2": 790, "y2": 800, "rot": 180}
},
"tags": []
}
}
wheelJoint = MultibodyComponents.PlanarMechanics.OneDOFRollingWheelJoint(radius = R) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 200, "y1": 700, "x2": 460, "y2": 960, "rot": 0}
},
"tags": []
}
}
IMU = MultibodyComponents.PlanarMechanics.AbsolutePosition(resolve_in_frame = MultibodyComponents.ResolveInFrame.World()) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 460, "y1": 540, "x2": 560, "y2": 640, "rot": 0}
},
"tags": []
}
}
square = BlockComponents.Sources.Square(amplitude = 0.05, frequency = 1 / 10) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -830, "y1": 600, "x2": -730, "y2": 700, "rot": 0}
},
"tags": []
}
}
wheelinertia = MultibodyComponents.PlanarMechanics.Body(I = Iw, radius = 0.01, m = m) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 390, "y1": 680, "x2": 530, "y2": 820, "rot": 0}
},
"tags": []
}
}
simplemotor = MultibodyComponents.PlanarMechanics.examples.dyad_balans.SimpleMotor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 620, "x2": 380, "y2": 720, "rot": 90}
},
"tags": []
}
}
angle_controller = BlockComponents.Continuous.LimPID(k = 0.487401, Ti = 0.0587352, Td = 0.0420526, Nd = 119.368, y_max = 0.1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 30, "y1": 620, "x2": 130, "y2": 720, "rot": 0}
},
"tags": []
}
}
pos_controller = BlockComponents.Continuous.LimPID(k = 0.0666576, Ti = 5.25024, Td = 4.81393, Nd = 4.76616, wd = 1, wp = 1, y_max = deg2rad(25.0)) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -260, "y1": 620, "x2": -160, "y2": 720, "rot": 0}
},
"tags": []
}
}
gain = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 170, "y1": 630, "x2": 250, "y2": 710, "rot": 0}
},
"tags": []
}
}
gain1 = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -120, "y1": 630, "x2": -40, "y2": 710, "rot": 0}
},
"tags": []
}
}
"""
refgen has 1 input (position reference) and 3 outputs:
y[1]: angle feedforward
y[2]: filtered position reference
y[3]: torque feedforward
"""
refgen = BlockComponents.Continuous.StateSpace(nx = ff_nx(), nu = 1, ny = 3, A = ff_A(), B = ff_B(), C = ff_C(), D = ff_D()) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -570, "y1": 600, "x2": -470, "y2": 700, "rot": 0}
},
"tags": []
}
}
angle_ff = MultibodyComponents.Selector(nu = 3) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 470, "x2": -310, "y2": 570, "rot": 0}
},
"tags": []
}
}
pos_ref = MultibodyComponents.Selector(nu = 3, index = 2) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 600, "x2": -310, "y2": 700, "rot": 0}
},
"tags": []
}
}
torque_ff = MultibodyComponents.Selector(nu = 3, index = 3) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -410, "y1": 340, "x2": -310, "y2": 440, "rot": 0}
},
"tags": []
}
}
"This negative gain is required since we are providing the angle reference signal to the `u_ff` connector of the outer controller which sits behind the negative gain `gain1`. We are thus effectively negating `gain1` with this `gain2`."
gain2 = BlockComponents.Math.Gain(k = -1) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -240, "y1": 540, "x2": -180, "y2": 600, "rot": 90}
},
"tags": []
}
}
body_mass = MultibodyComponents.PlanarMechanics.BodyShape(m = M, I = Ib, radius = 0.03, color = [0.2, 0.2, 0.2, 0.9], r = [0, 2 * L], r_cm = [0, L]) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 130, "y1": 130, "x2": 530, "y2": 530, "rot": 270}
},
"tags": []
}
}
mux1 = MultibodyComponents.PlanarMechanics.examples.dyad_balans.Mux1() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -700, "y1": 600, "x2": -600, "y2": 700, "rot": 0}
},
"tags": []
}
}
"Body mass"
parameter M::Mass = 0.1
"Wheel mass"
parameter m::Mass = 0.05
"Wheel radius"
parameter R::Length = 0.04
"Distance from wheel axis to body center of mass"
parameter L::Length = 0.08
"Body moment of inertia"
parameter Ib::Inertia = 2.5e-4
"Wheel + motor moment of inertia"
parameter Iw::Inertia = 1e-4
relations
initial body_mass.body.phi = 0.1
initial body_mass.body.w = 0.0
initial wheelinertia.phi = 0.0
initial wheelinertia.w = 0.0
connect(wheelinertia.frame_a, wheelJoint.frame_a) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 330, "y": 750}], "E": 2}],
"renderStyle": "standard"
}
}
connect(simplemotor.frame_b, wheelJoint.frame_a) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
y: analysis_point(IMU.phi, angle_controller.u_m)
y2: analysis_point(IMU.x, pos_controller.u_m)
u: analysis_point(angle_controller.y, gain.u)
u2: analysis_point(pos_controller.y, gain1.u)
connect(IMU.phi, angle_controller.u_m) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [
{"x": 660, "y": 621},
{"x": 660, "y": 1040},
{"x": 0, "y": 1040},
{"x": 0, "y": 693}
],
"E": 2
}
],
"renderStyle": "standard"
}
}
connect(IMU.x, pos_controller.u_m) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [
{"x": 710, "y": 560},
{"x": 710, "y": 1090},
{"x": -290, "y": 1090},
{"x": -290, "y": 693}
],
"E": 2
}
],
"renderStyle": "standard"
}
}
connect(pos_controller.y, gain1.u) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -134.25, "y": 671}, {"x": -134.25, "y": 670}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain1.y, angle_controller.u_s) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -10, "y": 670}, {"x": -10, "y": 647}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain.y, simplemotor.torqueinput) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(angle_controller.y, gain.u) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 150.5, "y": 671}, {"x": 150.5, "y": 670}], "E": 2}]
}
}
connect(pos_ref.y, pos_controller.u_s) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -290, "y": 650}, {"x": -290, "y": 647}], "E": 2}],
"renderStyle": "standard"
}
}
connect(refgen.y, pos_ref.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(refgen.y, torque_ff.u, angle_ff.u) {
"Dyad": {
"edges": [
{"S": 1, "M": [{"x": -445, "y": 650}], "E": -1},
{"S": -1, "M": [{"x": -445, "y": 390}], "E": 2},
{"S": 3, "M": [], "E": -1}
],
"junctions": [{"x": -445, "y": 520}],
"renderStyle": "standard"
}
}
connect(body_mass.frame_a, simplemotor.frame_a, IMU.frame_a) {
"Dyad": {
"edges": [
{
"S": 1,
"M": [{"x": 330.00000000000006, "y": 535}, {"x": 330, "y": 535}],
"E": -1
},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [], "E": -1}
],
"junctions": [{"x": 330, "y": 590}],
"renderStyle": "standard"
}
}
connect(square.y, mux1.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(mux1.y, refgen.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(angle_ff.y, gain2.u) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": -210, "y": 520}], "E": 2}],
"renderStyle": "standard"
}
}
connect(gain2.y, pos_controller.u_ff) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
connect(torque_ff.y, angle_controller.u_ff) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 80, "y": 390}], "E": 2}]
}
}
metadata {"Dyad": {"tests": {"case1": {"stop": 50}}}}
endTest Cases
julia
using MultibodyComponents
using DyadInterface: TransientAnalysis, rebuild_sol, ODEAlg
using ModelingToolkit: toggle_namespacing, get_initial_conditions, @named
using CSV, DataFrames, Plots
snapshotsdir = joinpath(dirname(dirname(pathof(MultibodyComponents))), "test", "snapshots")<< @setup-block not executed in draft mode >>Test Case case1
julia
@named model_case1 = MultibodyComponents.PlanarMechanics.examples.dyad_balans.DyadBalans2DFF()
model_case1 = toggle_namespacing(model_case1, false)
model_case1 = toggle_namespacing(model_case1, true)
result_case1 = TransientAnalysis(; model = model_case1, alg = ODEAlg.Auto(), start = 0e+0, stop = 5e+1, abstol=1e-6, reltol=1e-6)
sol_case1 = rebuild_sol(result_case1)<< @setup-block not executed in draft mode >>Related
Examples
Experiments
Analyses