Dyad

Appearance

LIBRARY
Examples.ACADOSMPCOmniPathFollowingExample.md

Examples.ACADOSMPCOmniPathFollowingExample ​

Path following of the same platform and the same planned path as ACADOSMPCOmniRobotExample, with the progress along the path as a state of the controller instead of a schedule fixed in advance.

ACADOSMPCOmniRobotExample samples the path at a nominal speed and hands the controller one position per shooting node, which makes the reference a trajectory: it says where the robot should be at each instant, and falling behind is an error to be corrected. Here the controller is given the path itself, inside its prediction model (OmniRobotPathModel), together with the progress    along it and the freedom to choose how fast    advances. The cost is the deviation from the path,      and     , driven to zero, so it says only where the robot should be on the path, and when to be there is the controller's business. That is the difference between a path and a trajectory, and it is the reason the controller has no reference input at all here (reference_tracking = false): the two deviations and the lateral velocity and yaw rate are all regulated to zero.

Three things follow from it:

  • The robot no longer starts out behind. In the tracking example the reference is already running at 0.4 m/s at t = 0 while the robot is at rest, which costs 0.09 m of error it spends the first second recovering. Here      simply stays near zero until the platform is up to speed. The mean deviation over the run is 0.9 mm against 10 mm.

  • The robot slows down where the path is hard. Through the two sharp arcs      drops to about 0.31 m/s and it recovers to 0.4 m/s on the straights, which is what a driver does and what the fixed schedule of the tracking example forbids. The path takes 12.4 s instead of the 11.8 s the schedule allots.

  • The trade-off between turning and translating is unchanged, because the cost that decides it is: reaches 0.25 m/s through the sharp step aside and stays below 0.04 m/s over the gentle return. Path following changes the timing, not the manoeuvre.

The progress state ​

Missing open brace for subscript

with    the fifth control, so Q2[5,5] prices departures from the nominal rate rather than the rate itself. That matters: with a cost on      alone the cheapest trajectory is to stand still, since the path point then stays under the robot and the deviation is zero for free. Q2[5,5] = 1 is enough to rule that out here and still leave the 20 % slowdown at the sharp arcs; an order of magnitude more and the robot holds the nominal rate through them instead.    is bounded below by Missing open brace for subscript, so the progress never runs backwards. The state itself is integrated in the loop by PathProgress, because it belongs to the controller and not to the robot.

The prediction model ​

OmniRobotPathModel is not part of the loop, so the controller is given its component constructor with the arguments of this example, component_constructor(OmniRobotPathModel, m = m, a_max = a_max, v_nom = v_nom): the platform it predicts with is the plant, and the nominal rate of progress is the one PathProgress integrates.

The path as an input ​

The per-segment tables of the path are the online parameters p of the controller (tunable_parameters), not constants of the model, which is how a planner works: it replans as the robot moves, and a new plan is a new p and nothing else – no rebuilt solver. Here the plan is fixed and p is wired from a parameter through the p input (parameter_input = true), so ODEProblem(ssys, [ssys.path_p => another_path], ...) is all it takes to drive the same loop along a different path. Verified in test/test_acados_mpc.jl, which also switches the path mid-run.

Usage ​

MPCComponents.Examples.ACADOSMPCOmniPathFollowingExample(path_p=omni_path_parameters(), obstacle_x=1.5, obstacle_y=0.0, obstacle_radius=0.22)

Parameters: ​

NameDescriptionUnitsDefault value
NpPrediction horizon (number of shooting intervals)–20
TsSample interval, the length of the first shooting interval–0.02
TfPrediction horizon in seconds, spanned by the Np shooting intervals–2.0
v_nomRate of progress along the path that costs nothing; departures from it are priced by Q2[5,5]–0.4
output_trajectoriesRecord the predicted trajectories and solver residuals of the controller (for mpc_gui)–false
mMass of the platform–5.38
a_maxAcceleration limit of the platform–1.0
v_maxSpeed limit of the platform in both body directions–0.8
om_maxYaw-rate limit of the platform–3.14
f_maxForce bound of one wheel, the acceleration limit spread over the four wheels (derived)–m * a_max / 4
time_stepsLengths of the shooting intervals, growing linearly from Ts to cover Tf (derived)–linear_time...Ts, Np, Tf)
npNumber of online parameters carrying the planned path (derived)–omni_path_p...ter_count()
path_pThe planned path: the per-segment tables the controller takes as online parameters–omni_path_parameters()
obstacle_xPosition along the corridor of the obstacle the path was planned around–1.5
obstacle_yPosition across the corridor of the obstacle–0.0
obstacle_radiusRadius of the obstacle–0.22

Variables ​

NameDescriptionUnits
pxPosition along the corridor–
pyPosition across the corridor–
thHeading–
vyLateral velocity in the body frame–
omYaw rate–
sigmaProgress along the planned path–
sigma_dotRate of progress along the planned path–
u1Drive force of the front-left wheel–
flagacados status–

Behavior ​

Behavior of this component cannot be rendered because it includes path variables.

Source ​

dyad
"""
Path following of the same platform and the same planned path as `ACADOSMPCOmniRobotExample`, with the
progress along the path as a state of the controller instead of a schedule fixed in advance.

`ACADOSMPCOmniRobotExample` samples the path at a nominal speed and hands the controller one position
per shooting node, which makes the reference a trajectory: it says where the robot should be at each
instant, and falling behind is an error to be corrected. Here the controller is given the path itself,
inside its prediction model (`OmniRobotPathModel`), together with the progress ``\\sigma`` along it and
the freedom to choose how fast ``\\sigma`` advances. The cost is the deviation from the path,
``e_x = p_x - x(\\sigma)`` and ``e_y = p_y - y(\\sigma)``, driven to zero, so it says only where the
robot should be *on the path*, and when to be there is the controller's business. That is the
difference between a path and a trajectory, and it is the reason the controller has no reference input
at all here (`reference_tracking = false`): the two deviations and the lateral velocity and yaw rate
are all regulated to zero.

Three things follow from it:

  * The robot no longer starts out behind. In the tracking example the reference is already running at
    0.4 m/s at t = 0 while the robot is at rest, which costs 0.09 m of error it spends the first second
    recovering. Here ``\\dot\\sigma`` simply stays near zero until the platform is up to speed. The mean
    deviation over the run is 0.9 mm against 10 mm.
  * The robot slows down where the path is hard. Through the two sharp arcs ``\\dot\\sigma`` drops to
    about 0.31 m/s and it recovers to 0.4 m/s on the straights, which is what a driver does and what
    the fixed schedule of the tracking example forbids. The path takes 12.4 s instead of the 11.8 s the
    schedule allots.
  * The trade-off between turning and translating is unchanged, because the cost that decides it is:
    ``|v_y|`` reaches 0.25 m/s through the sharp step aside and stays below 0.04 m/s over the gentle
    return. Path following changes the timing, not the manoeuvre.

# The progress state
``\\dot\\sigma = v_\\mathrm{nom} + \\Delta v`` with ``\\Delta v`` the fifth control, so `Q2[5,5]` prices
departures from the nominal rate rather than the rate itself. That matters: with a cost on
``\\dot\\sigma`` alone the cheapest trajectory is to stand still, since the path point then stays under
the robot and the deviation is zero for free. `Q2[5,5] = 1` is enough to rule that out here and still
leave the 20 % slowdown at the sharp arcs; an order of magnitude more and the robot holds the nominal
rate through them instead. ``\\Delta v`` is bounded below by ``-v_\\mathrm{nom}``, so the progress never
runs backwards. The state itself is integrated in the loop by `PathProgress`, because it belongs to the
controller and not to the robot.

# The prediction model
`OmniRobotPathModel` is not part of the loop, so the controller is given its component constructor
with the arguments of this example, `component_constructor(OmniRobotPathModel, m = m, a_max = a_max,
v_nom = v_nom)`: the platform it predicts with is the plant, and the nominal rate of progress is the
one `PathProgress` integrates.

# The path as an input
The per-segment tables of the path are the online parameters `p` of the controller
(`tunable_parameters`), not constants of the model, which is how a planner works: it replans as the
robot moves, and a new plan is a new `p` and nothing else -- no rebuilt solver. Here the plan is fixed
and `p` is wired from a parameter through the `p` input (`parameter_input = true`), so
`ODEProblem(ssys, [ssys.path_p => another_path], ...)` is all it takes to drive the same loop along a
different path. Verified in `test/test_acados_mpc.jl`, which also switches the path mid-run.
"""
example component ACADOSMPCOmniPathFollowingExample
  periodicclock = DiscreteComponents.PeriodicClock(final dt = Ts) {
    "Dyad": {"placement": {"diagram": {"x1": 300, "y1": 140, "x2": 400, "y2": 240}}}
  }
  plant = MPCComponents.Examples.OmniRobotPlant(m = m, a_max = a_max) {
    "Dyad": {"placement": {"diagram": {"x1": 700, "y1": 300, "x2": 800, "y2": 400}}}
  }
  "Samplers of the six state variables of the platform and of the progress along the path"
  s = [DiscreteComponents.Sampler() for i in 1:7] {
    "Dyad": {
      "placement": {"diagram": {"x1": 700, "y1": 600, "x2": 800, "y2": 700, "rot": 180}}
    }
  }
  "Holds of the four wheel forces and of the progress rate"
  zoh = [DiscreteComponents.ZeroOrderHold() for i in 1:5] {
    "Dyad": {"placement": {"diagram": {"x1": 500, "y1": 300, "x2": 600, "y2": 400}}}
  }
  "The progress along the path, a state of the controller integrated in the loop"
  progress = MPCComponents.Examples.PathProgress(final v_nom = v_nom) {
    "Dyad": {"placement": {"diagram": {"x1": 700, "y1": 450, "x2": 800, "y2": 550}}}
  }
  mpc = MPCComponents.ACADOSMPC(final dynamics = component_constructor(OmniRobotPathModel, m = m, a_max = a_max, v_nom = v_nom), inputs = ["f1", "f2", "f3", "f4", "ds"], tunable_parameters = ["seg_curvature", "seg_length", "seg_radius"], parameter_input = true, state_variables = ["px", "py", "th", "vx", "vy", "om", "s"], outputs = ["ex", "ey", "vy", "om"], final Ts = Ts, final Np = Np, final time_steps = time_steps, reference_tracking = false, Q1 = diagonal([200.0, 200.0, 1.0, 0.1]), Q2 = diagonal([0.01, 0.01, 0.01, 0.01, 1.0]), final umin = [-f_max, -f_max, -f_max, -f_max, -v_nom], final umax = [f_max, f_max, f_max, f_max, v_nom], constrained = ["vx", "vy", "om"], final constrained_min = [-v_max, -v_max, -om_max], final constrained_max = [v_max, v_max, om_max], integrator_steps = 1, final output_trajectories = output_trajectories, nlp_solver = MPCComponents.ACADOSSolver.SQP_RTI(), qp_cond_N = 5) {
    "Dyad": {"placement": {"diagram": {"x1": 300, "y1": 300, "x2": 400, "y2": 400}}}
  }
  "Prediction horizon (number of shooting intervals)"
  structural parameter Np::Integer = 20
  "Sample interval, the length of the first shooting interval"
  structural parameter Ts::Real = 0.02
  "Prediction horizon in seconds, spanned by the Np shooting intervals"
  structural parameter Tf::Real = 2.0
  "Rate of progress along the path that costs nothing; departures from it are priced by Q2[5,5]"
  structural parameter v_nom::Real = 0.4
  "Record the predicted trajectories and solver residuals of the controller (for mpc_gui)"
  structural parameter output_trajectories::Boolean = false
  "Mass of the platform"
  structural parameter m::Real = 5.38
  "Acceleration limit of the platform"
  structural parameter a_max::Real = 1.0
  "Speed limit of the platform in both body directions"
  structural parameter v_max::Real = 0.8
  "Yaw-rate limit of the platform"
  structural parameter om_max::Real = 3.14
  "Force bound of one wheel, the acceleration limit spread over the four wheels (derived)"
  final structural parameter f_max::Real = m * a_max / 4
  "Lengths of the shooting intervals, growing linearly from Ts to cover Tf (derived)"
  final structural parameter time_steps::Real[:] = linear_time_steps(Ts, Np, Tf)
  "Number of online parameters carrying the planned path (derived)"
  final structural parameter np::Integer = omni_path_parameter_count()
  "The planned path: the per-segment tables the controller takes as online parameters"
  parameter path_p::Real[np] = omni_path_parameters()
  "Position along the corridor of the obstacle the path was planned around"
  parameter obstacle_x::Real = 1.5
  "Position across the corridor of the obstacle"
  parameter obstacle_y::Real = 0.0
  "Radius of the obstacle"
  parameter obstacle_radius::Real = 0.22
  "Position along the corridor"
  variable px::Real
  "Position across the corridor"
  variable py::Real
  "Heading"
  variable th::Real
  "Lateral velocity in the body frame"
  variable vy::Real
  "Yaw rate"
  variable om::Real
  "Progress along the planned path"
  variable sigma::Real
  "Rate of progress along the planned path"
  variable sigma_dot::Real
  "Drive force of the front-left wheel"
  variable u1::Real
  "acados status"
  variable flag::Real
relations
  px = plant.px
  py = plant.py
  th = plant.th
  vy = plant.vy
  om = plant.om
  sigma = progress.s
  sigma_dot = v_nom + mpc.u[5]
  u1 = mpc.u[1]
  flag = mpc.exitflag
  connect(plant.px_out, s[1].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 910, "y": 310}, {"x": 910, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.py_out, s[2].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 900, "y": 325}, {"x": 900, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.th_out, s[3].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 890, "y": 340}, {"x": 890, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.vx_out, s[4].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 880, "y": 355}, {"x": 880, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.vy_out, s[5].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 870, "y": 370}, {"x": 870, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.om_out, s[6].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 860, "y": 385}, {"x": 860, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(progress.s_out, s[7].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 850, "y": 500}, {"x": 850, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(s[1].y, mpc.x[1], periodicclock.y) {
    "Dyad": {
      "edges": [
        {"S": 1, "M": [{"x": 270, "y": 650}, {"x": 270, "y": 383.5}], "E": 2},
        {"S": 2, "M": [{"x": 270, "y": 383.5}, {"x": 270, "y": 190}], "E": 3}
      ],
      "path": {}
    }
  }
  for i in 2:7
    connect(s[i].y, mpc.x[i])
  end
  for i in 1:5
    connect(mpc.u[i], zoh[i].u)
  end
  for i in 1:np
    mpc.p[i] = path_p[i]
  end
  connect(zoh[1].y, plant.f1) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 315}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[2].y, plant.f2) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 338}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[3].y, plant.f3) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 632.3333333333333, "y": 350},
            {"x": 632.3333333333333, "y": 360},
            {"x": 663.6666666666666, "y": 360},
            {"x": 663.6666666666666, "y": 361}
          ],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[4].y, plant.f4) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 384}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[5].y, progress.ds) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 601, "y": 360}, {"x": 620, "y": 360}, {"x": 620, "y": 500}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
metadata {"Dyad": {"doc": {"behavior": false}}}
end
Flattened Source
dyad
"""
Path following of the same platform and the same planned path as `ACADOSMPCOmniRobotExample`, with the
progress along the path as a state of the controller instead of a schedule fixed in advance.

`ACADOSMPCOmniRobotExample` samples the path at a nominal speed and hands the controller one position
per shooting node, which makes the reference a trajectory: it says where the robot should be at each
instant, and falling behind is an error to be corrected. Here the controller is given the path itself,
inside its prediction model (`OmniRobotPathModel`), together with the progress ``\\sigma`` along it and
the freedom to choose how fast ``\\sigma`` advances. The cost is the deviation from the path,
``e_x = p_x - x(\\sigma)`` and ``e_y = p_y - y(\\sigma)``, driven to zero, so it says only where the
robot should be *on the path*, and when to be there is the controller's business. That is the
difference between a path and a trajectory, and it is the reason the controller has no reference input
at all here (`reference_tracking = false`): the two deviations and the lateral velocity and yaw rate
are all regulated to zero.

Three things follow from it:

  * The robot no longer starts out behind. In the tracking example the reference is already running at
    0.4 m/s at t = 0 while the robot is at rest, which costs 0.09 m of error it spends the first second
    recovering. Here ``\\dot\\sigma`` simply stays near zero until the platform is up to speed. The mean
    deviation over the run is 0.9 mm against 10 mm.
  * The robot slows down where the path is hard. Through the two sharp arcs ``\\dot\\sigma`` drops to
    about 0.31 m/s and it recovers to 0.4 m/s on the straights, which is what a driver does and what
    the fixed schedule of the tracking example forbids. The path takes 12.4 s instead of the 11.8 s the
    schedule allots.
  * The trade-off between turning and translating is unchanged, because the cost that decides it is:
    ``|v_y|`` reaches 0.25 m/s through the sharp step aside and stays below 0.04 m/s over the gentle
    return. Path following changes the timing, not the manoeuvre.

# The progress state
``\\dot\\sigma = v_\\mathrm{nom} + \\Delta v`` with ``\\Delta v`` the fifth control, so `Q2[5,5]` prices
departures from the nominal rate rather than the rate itself. That matters: with a cost on
``\\dot\\sigma`` alone the cheapest trajectory is to stand still, since the path point then stays under
the robot and the deviation is zero for free. `Q2[5,5] = 1` is enough to rule that out here and still
leave the 20 % slowdown at the sharp arcs; an order of magnitude more and the robot holds the nominal
rate through them instead. ``\\Delta v`` is bounded below by ``-v_\\mathrm{nom}``, so the progress never
runs backwards. The state itself is integrated in the loop by `PathProgress`, because it belongs to the
controller and not to the robot.

# The prediction model
`OmniRobotPathModel` is not part of the loop, so the controller is given its component constructor
with the arguments of this example, `component_constructor(OmniRobotPathModel, m = m, a_max = a_max,
v_nom = v_nom)`: the platform it predicts with is the plant, and the nominal rate of progress is the
one `PathProgress` integrates.

# The path as an input
The per-segment tables of the path are the online parameters `p` of the controller
(`tunable_parameters`), not constants of the model, which is how a planner works: it replans as the
robot moves, and a new plan is a new `p` and nothing else -- no rebuilt solver. Here the plan is fixed
and `p` is wired from a parameter through the `p` input (`parameter_input = true`), so
`ODEProblem(ssys, [ssys.path_p => another_path], ...)` is all it takes to drive the same loop along a
different path. Verified in `test/test_acados_mpc.jl`, which also switches the path mid-run.
"""
example component ACADOSMPCOmniPathFollowingExample
  periodicclock = DiscreteComponents.PeriodicClock(final dt = Ts) {
    "Dyad": {"placement": {"diagram": {"x1": 300, "y1": 140, "x2": 400, "y2": 240}}}
  }
  plant = MPCComponents.Examples.OmniRobotPlant(m = m, a_max = a_max) {
    "Dyad": {"placement": {"diagram": {"x1": 700, "y1": 300, "x2": 800, "y2": 400}}}
  }
  "Samplers of the six state variables of the platform and of the progress along the path"
  s = [DiscreteComponents.Sampler() for i in 1:7] {
    "Dyad": {
      "placement": {"diagram": {"x1": 700, "y1": 600, "x2": 800, "y2": 700, "rot": 180}}
    }
  }
  "Holds of the four wheel forces and of the progress rate"
  zoh = [DiscreteComponents.ZeroOrderHold() for i in 1:5] {
    "Dyad": {"placement": {"diagram": {"x1": 500, "y1": 300, "x2": 600, "y2": 400}}}
  }
  "The progress along the path, a state of the controller integrated in the loop"
  progress = MPCComponents.Examples.PathProgress(final v_nom = v_nom) {
    "Dyad": {"placement": {"diagram": {"x1": 700, "y1": 450, "x2": 800, "y2": 550}}}
  }
  mpc = MPCComponents.ACADOSMPC(final dynamics = component_constructor(OmniRobotPathModel, m = m, a_max = a_max, v_nom = v_nom), inputs = ["f1", "f2", "f3", "f4", "ds"], tunable_parameters = ["seg_curvature", "seg_length", "seg_radius"], parameter_input = true, state_variables = ["px", "py", "th", "vx", "vy", "om", "s"], outputs = ["ex", "ey", "vy", "om"], final Ts = Ts, final Np = Np, final time_steps = time_steps, reference_tracking = false, Q1 = diagonal([200.0, 200.0, 1.0, 0.1]), Q2 = diagonal([0.01, 0.01, 0.01, 0.01, 1.0]), final umin = [-f_max, -f_max, -f_max, -f_max, -v_nom], final umax = [f_max, f_max, f_max, f_max, v_nom], constrained = ["vx", "vy", "om"], final constrained_min = [-v_max, -v_max, -om_max], final constrained_max = [v_max, v_max, om_max], integrator_steps = 1, final output_trajectories = output_trajectories, nlp_solver = MPCComponents.ACADOSSolver.SQP_RTI(), qp_cond_N = 5) {
    "Dyad": {"placement": {"diagram": {"x1": 300, "y1": 300, "x2": 400, "y2": 400}}}
  }
  "Prediction horizon (number of shooting intervals)"
  structural parameter Np::Integer = 20
  "Sample interval, the length of the first shooting interval"
  structural parameter Ts::Real = 0.02
  "Prediction horizon in seconds, spanned by the Np shooting intervals"
  structural parameter Tf::Real = 2.0
  "Rate of progress along the path that costs nothing; departures from it are priced by Q2[5,5]"
  structural parameter v_nom::Real = 0.4
  "Record the predicted trajectories and solver residuals of the controller (for mpc_gui)"
  structural parameter output_trajectories::Boolean = false
  "Mass of the platform"
  structural parameter m::Real = 5.38
  "Acceleration limit of the platform"
  structural parameter a_max::Real = 1.0
  "Speed limit of the platform in both body directions"
  structural parameter v_max::Real = 0.8
  "Yaw-rate limit of the platform"
  structural parameter om_max::Real = 3.14
  "Force bound of one wheel, the acceleration limit spread over the four wheels (derived)"
  final structural parameter f_max::Real = m * a_max / 4
  "Lengths of the shooting intervals, growing linearly from Ts to cover Tf (derived)"
  final structural parameter time_steps::Real[:] = linear_time_steps(Ts, Np, Tf)
  "Number of online parameters carrying the planned path (derived)"
  final structural parameter np::Integer = omni_path_parameter_count()
  "The planned path: the per-segment tables the controller takes as online parameters"
  parameter path_p::Real[np] = omni_path_parameters()
  "Position along the corridor of the obstacle the path was planned around"
  parameter obstacle_x::Real = 1.5
  "Position across the corridor of the obstacle"
  parameter obstacle_y::Real = 0.0
  "Radius of the obstacle"
  parameter obstacle_radius::Real = 0.22
  "Position along the corridor"
  variable px::Real
  "Position across the corridor"
  variable py::Real
  "Heading"
  variable th::Real
  "Lateral velocity in the body frame"
  variable vy::Real
  "Yaw rate"
  variable om::Real
  "Progress along the planned path"
  variable sigma::Real
  "Rate of progress along the planned path"
  variable sigma_dot::Real
  "Drive force of the front-left wheel"
  variable u1::Real
  "acados status"
  variable flag::Real
relations
  px = plant.px
  py = plant.py
  th = plant.th
  vy = plant.vy
  om = plant.om
  sigma = progress.s
  sigma_dot = v_nom + mpc.u[5]
  u1 = mpc.u[1]
  flag = mpc.exitflag
  connect(plant.px_out, s[1].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 910, "y": 310}, {"x": 910, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.py_out, s[2].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 900, "y": 325}, {"x": 900, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.th_out, s[3].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 890, "y": 340}, {"x": 890, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.vx_out, s[4].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 880, "y": 355}, {"x": 880, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.vy_out, s[5].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 870, "y": 370}, {"x": 870, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(plant.om_out, s[6].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 860, "y": 385}, {"x": 860, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(progress.s_out, s[7].u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 850, "y": 500}, {"x": 850, "y": 650}], "E": 2}],
      "path": {}
    }
  }
  connect(s[1].y, mpc.x[1], periodicclock.y) {
    "Dyad": {
      "edges": [
        {"S": 1, "M": [{"x": 270, "y": 650}, {"x": 270, "y": 383.5}], "E": 2},
        {"S": 2, "M": [{"x": 270, "y": 383.5}, {"x": 270, "y": 190}], "E": 3}
      ],
      "path": {}
    }
  }
  for i in 2:7
    connect(s[i].y, mpc.x[i])
  end
  for i in 1:5
    connect(mpc.u[i], zoh[i].u)
  end
  for i in 1:np
    mpc.p[i] = path_p[i]
  end
  connect(zoh[1].y, plant.f1) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 315}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[2].y, plant.f2) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 338}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[3].y, plant.f3) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 632.3333333333333, "y": 350},
            {"x": 632.3333333333333, "y": 360},
            {"x": 663.6666666666666, "y": 360},
            {"x": 663.6666666666666, "y": 361}
          ],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[4].y, plant.f4) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 632.3333333333334, "y": 350}, {"x": 632.3333333333334, "y": 384}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
  connect(zoh[5].y, progress.ds) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 601, "y": 360}, {"x": 620, "y": 360}, {"x": 620, "y": 500}],
          "E": 2
        }
      ],
      "path": {}
    }
  }
metadata {"Dyad": {"doc": {"behavior": false}}}
end


Test Cases ​

No test cases defined.

  • Examples

  • Experiments

  • Analyses