Dyad

Appearance

LIBRARY
Examples.OmniPathPreview.md

Examples.OmniPathPreview ​

Preview of the planned path of ACADOSMPCOmniRobotExample: the two coordinates of the path at each of the Np shooting nodes of the controller's grid, to be sampled and fed to the previewed r input of ACADOSMPC.

The outputs are continuous and the example samples them with one DiscreteComponents.Sampler each. Emitting them on the controller's clock from inside this component would spare those samplers, but a component that both creates a continuous partition and samples it is rejected by the clock analysis (JuliaComputing/dyad-lang#2107), whichever way the clock parameter is declared.

Node j is the end of shooting interval j, not the j-th multiple of Ts, so the path is sampled at the cumulative lengths of the same linear_time_steps(Ts, Np, Tf) grid the controller is built with; Ts, Np and Tf must be the ones given to the controller. A DelayLine taps a signal every Ts and therefore cannot address the nodes of a stretched grid.

The planner itself is not part of the example. Its path is the line-arc sequence OMNI_PATH_SEGMENTS, whose per-segment constants arrive as the parameters below, and it is sampled along its arc length at speed, which is feasible whatever the shape of the path (a constant-speed sample of a corner would not be, were the path parametrized by one of its coordinates). The preview carries positions only – no velocity and no heading – so the controller is free to choose how to traverse the path.

The pose at the arc length is the sum over the segments of what each of them contributes, each clipped to the progress reached in it,       :

Misplaced &

with    the heading and the arc length at which segment starts. Both cases are exact for a segment of constant curvature, so no integration and no search over the segments is needed; seg_straight selects between them and is what keeps the straight segments, whose radius is infinite, out of the arc formula.

Usage ​

MPCComponents.Examples.OmniPathPreview(speed=0.4, node_times=cumsum(linear_time_steps(Ts, Np, Tf)), seg_start=omni_path_starts(), seg_length=omni_path_lengths(), seg_curvature=omni_path_curvatures(), seg_radius=omni_path_radii(), seg_straight=omni_path_straight(), seg_heading=omni_path_headings(), path_length=sum(omni_path_lengths()))

Parameters: ​

NameDescriptionUnitsDefault value
NpPrediction horizon (number of shooting intervals) of the controller–20
TsLength of the first shooting interval, the clock period of the controller–0.02
TfHorizon spanned by the shooting grid–2.0
nsegNumber of segments of the planned path (derived)–omni_path_segment_count()
nboundNumber of segment boundaries of the planned path (derived)–nseg + 1
speedSpeed at which the planned path is sampled–0.4

Connectors ​

  • px - This connector represents a real signal as an output from a component (RealOutput)

  • py - This connector represents a real signal as an output from a component (RealOutput)

Variables ​

NameDescriptionUnits
sArc length reached at each shooting node, held at the end of the path–
dArc length spent in each segment at each shooting node–
xkCoordinate along the corridor after the first k segments–
ykCoordinate across the corridor after the first k segments–

Behavior ​

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

Source ​

dyad
"""
Preview of the planned path of `ACADOSMPCOmniRobotExample`: the two coordinates of the path at each
of the `Np` shooting nodes of the controller's grid, to be sampled and fed to the previewed `r` input
of `ACADOSMPC`.

The outputs are continuous and the example samples them with one `DiscreteComponents.Sampler` each.
Emitting them on the controller's clock from inside this component would spare those samplers, but a
component that both creates a continuous partition and samples it is rejected by the clock analysis
(JuliaComputing/dyad-lang#2107), whichever way the clock parameter is declared.

Node `j` is the end of shooting interval `j`, not the j-th multiple of `Ts`, so the path is sampled at
the cumulative lengths of the same `linear_time_steps(Ts, Np, Tf)` grid the controller is built with;
`Ts`, `Np` and `Tf` must be the ones given to the controller. A `DelayLine` taps a signal every `Ts`
and therefore cannot address the nodes of a stretched grid.

The planner itself is not part of the example. Its path is the line-arc sequence
`OMNI_PATH_SEGMENTS`, whose per-segment constants arrive as the parameters below, and it is sampled
along its arc length at `speed`, which is feasible whatever the shape of the path (a constant-speed
sample of a corner would not be, were the path parametrized by one of its coordinates). The preview
carries positions only -- no velocity and no heading -- so the controller is free to choose how to
traverse the path.

The pose at the arc length ``s`` is the sum over the segments of what each of them contributes, each
clipped to the progress reached in it, ``d_k = \\mathrm{clamp}(s - s_k, 0, \\ell_k)``:

```math
\\begin{bmatrix} x \\\\ y \\end{bmatrix} = \\sum_k \\begin{cases}
d_k \\begin{bmatrix} \\cos\\psi_k \\\\ \\sin\\psi_k \\end{bmatrix} & \\kappa_k = 0 \\\\
R_k \\begin{bmatrix} \\sin(\\psi_k + \\kappa_k d_k) - \\sin\\psi_k \\\\
                     \\cos\\psi_k - \\cos(\\psi_k + \\kappa_k d_k) \\end{bmatrix} & \\kappa_k \\ne 0
\\end{cases}
```

with ``\\psi_k`` the heading and ``s_k`` the arc length at which segment ``k`` starts. Both cases are
exact for a segment of constant curvature, so no integration and no search over the segments is
needed; `seg_straight` selects between them and is what keeps the straight segments, whose radius is
infinite, out of the arc formula.
"""
component OmniPathPreview
  "Coordinate of the path along the corridor at each shooting node"
  px = [RealOutput() for i in 1:Np] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 250, "x2": 1100, "y2": 350}}}
  }
  "Coordinate of the path across the corridor at each shooting node"
  py = [RealOutput() for i in 1:Np] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 650, "x2": 1100, "y2": 750}}}
  }
  "Prediction horizon (number of shooting intervals) of the controller"
  structural parameter Np::Integer = 20
  "Length of the first shooting interval, the clock period of the controller"
  structural parameter Ts::Real = 0.02
  "Horizon spanned by the shooting grid"
  structural parameter Tf::Real = 2.0
  "Speed at which the planned path is sampled"
  parameter speed::Real = 0.4
  "Number of segments of the planned path (derived)"
  final structural parameter nseg::Integer = omni_path_segment_count()
  "Number of segment boundaries of the planned path (derived)"
  final structural parameter nbound::Integer = nseg + 1
  "Time from the current tick to each shooting node (derived)"
  final parameter node_times::Real[Np] = cumsum(linear_time_steps(Ts, Np, Tf))
  "Arc length at which each segment starts (derived)"
  final parameter seg_start::Real[nseg] = omni_path_starts()
  "Length of each segment (derived)"
  final parameter seg_length::Real[nseg] = omni_path_lengths()
  "Curvature of each segment (derived)"
  final parameter seg_curvature::Real[nseg] = omni_path_curvatures()
  "Radius of each segment, zero for a straight one (derived)"
  final parameter seg_radius::Real[nseg] = omni_path_radii()
  "One for a straight segment, zero for an arc (derived)"
  final parameter seg_straight::Real[nseg] = omni_path_straight()
  "Heading at which each segment starts (derived)"
  final parameter seg_heading::Real[nseg] = omni_path_headings()
  "Total length of the planned path (derived)"
  final parameter path_length::Real = sum(omni_path_lengths())
  "Arc length reached at each shooting node, held at the end of the path"
  variable s::Real[Np]
  "Arc length spent in each segment at each shooting node"
  variable d::Real[Np, nseg]
  "Coordinate along the corridor after the first k segments"
  variable xk::Real[Np, nbound]
  "Coordinate across the corridor after the first k segments"
  variable yk::Real[Np, nbound]
relations
  for j in 1:Np
    s[j] = min(speed * (time + node_times[j]), path_length)
    xk[j, 1] = 0
    yk[j, 1] = 0
    for k in 1:nseg
      d[j, k] = max(min(s[j] - seg_start[k], seg_length[k]), 0)
      xk[j, k + 1] = xk[j, k] + seg_straight[k] * d[j, k] * cos(seg_heading[k]) + seg_radius[k] * (sin(seg_heading[k] + seg_curvature[k] * d[j, k]) - sin(seg_heading[k]))
      yk[j, k + 1] = yk[j, k] + seg_straight[k] * d[j, k] * sin(seg_heading[k]) + seg_radius[k] * (cos(seg_heading[k]) - cos(seg_heading[k] + seg_curvature[k] * d[j, k]))
    end
    px[j] = xk[j, nbound]
    py[j] = yk[j, nbound]
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/OmniPathPreview.svg"},
    "doc": {"behavior": false}
  }
}
end
Flattened Source
dyad
"""
Preview of the planned path of `ACADOSMPCOmniRobotExample`: the two coordinates of the path at each
of the `Np` shooting nodes of the controller's grid, to be sampled and fed to the previewed `r` input
of `ACADOSMPC`.

The outputs are continuous and the example samples them with one `DiscreteComponents.Sampler` each.
Emitting them on the controller's clock from inside this component would spare those samplers, but a
component that both creates a continuous partition and samples it is rejected by the clock analysis
(JuliaComputing/dyad-lang#2107), whichever way the clock parameter is declared.

Node `j` is the end of shooting interval `j`, not the j-th multiple of `Ts`, so the path is sampled at
the cumulative lengths of the same `linear_time_steps(Ts, Np, Tf)` grid the controller is built with;
`Ts`, `Np` and `Tf` must be the ones given to the controller. A `DelayLine` taps a signal every `Ts`
and therefore cannot address the nodes of a stretched grid.

The planner itself is not part of the example. Its path is the line-arc sequence
`OMNI_PATH_SEGMENTS`, whose per-segment constants arrive as the parameters below, and it is sampled
along its arc length at `speed`, which is feasible whatever the shape of the path (a constant-speed
sample of a corner would not be, were the path parametrized by one of its coordinates). The preview
carries positions only -- no velocity and no heading -- so the controller is free to choose how to
traverse the path.

The pose at the arc length ``s`` is the sum over the segments of what each of them contributes, each
clipped to the progress reached in it, ``d_k = \\mathrm{clamp}(s - s_k, 0, \\ell_k)``:

```math
\\begin{bmatrix} x \\\\ y \\end{bmatrix} = \\sum_k \\begin{cases}
d_k \\begin{bmatrix} \\cos\\psi_k \\\\ \\sin\\psi_k \\end{bmatrix} & \\kappa_k = 0 \\\\
R_k \\begin{bmatrix} \\sin(\\psi_k + \\kappa_k d_k) - \\sin\\psi_k \\\\
                     \\cos\\psi_k - \\cos(\\psi_k + \\kappa_k d_k) \\end{bmatrix} & \\kappa_k \\ne 0
\\end{cases}
```

with ``\\psi_k`` the heading and ``s_k`` the arc length at which segment ``k`` starts. Both cases are
exact for a segment of constant curvature, so no integration and no search over the segments is
needed; `seg_straight` selects between them and is what keeps the straight segments, whose radius is
infinite, out of the arc formula.
"""
component OmniPathPreview
  "Coordinate of the path along the corridor at each shooting node"
  px = [RealOutput() for i in 1:Np] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 250, "x2": 1100, "y2": 350}}}
  }
  "Coordinate of the path across the corridor at each shooting node"
  py = [RealOutput() for i in 1:Np] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 650, "x2": 1100, "y2": 750}}}
  }
  "Prediction horizon (number of shooting intervals) of the controller"
  structural parameter Np::Integer = 20
  "Length of the first shooting interval, the clock period of the controller"
  structural parameter Ts::Real = 0.02
  "Horizon spanned by the shooting grid"
  structural parameter Tf::Real = 2.0
  "Speed at which the planned path is sampled"
  parameter speed::Real = 0.4
  "Number of segments of the planned path (derived)"
  final structural parameter nseg::Integer = omni_path_segment_count()
  "Number of segment boundaries of the planned path (derived)"
  final structural parameter nbound::Integer = nseg + 1
  "Time from the current tick to each shooting node (derived)"
  final parameter node_times::Real[Np] = cumsum(linear_time_steps(Ts, Np, Tf))
  "Arc length at which each segment starts (derived)"
  final parameter seg_start::Real[nseg] = omni_path_starts()
  "Length of each segment (derived)"
  final parameter seg_length::Real[nseg] = omni_path_lengths()
  "Curvature of each segment (derived)"
  final parameter seg_curvature::Real[nseg] = omni_path_curvatures()
  "Radius of each segment, zero for a straight one (derived)"
  final parameter seg_radius::Real[nseg] = omni_path_radii()
  "One for a straight segment, zero for an arc (derived)"
  final parameter seg_straight::Real[nseg] = omni_path_straight()
  "Heading at which each segment starts (derived)"
  final parameter seg_heading::Real[nseg] = omni_path_headings()
  "Total length of the planned path (derived)"
  final parameter path_length::Real = sum(omni_path_lengths())
  "Arc length reached at each shooting node, held at the end of the path"
  variable s::Real[Np]
  "Arc length spent in each segment at each shooting node"
  variable d::Real[Np, nseg]
  "Coordinate along the corridor after the first k segments"
  variable xk::Real[Np, nbound]
  "Coordinate across the corridor after the first k segments"
  variable yk::Real[Np, nbound]
relations
  for j in 1:Np
    s[j] = min(speed * (time + node_times[j]), path_length)
    xk[j, 1] = 0
    yk[j, 1] = 0
    for k in 1:nseg
      d[j, k] = max(min(s[j] - seg_start[k], seg_length[k]), 0)
      xk[j, k + 1] = xk[j, k] + seg_straight[k] * d[j, k] * cos(seg_heading[k]) + seg_radius[k] * (sin(seg_heading[k] + seg_curvature[k] * d[j, k]) - sin(seg_heading[k]))
      yk[j, k + 1] = yk[j, k] + seg_straight[k] * d[j, k] * sin(seg_heading[k]) + seg_radius[k] * (cos(seg_heading[k]) - cos(seg_heading[k] + seg_curvature[k] * d[j, k]))
    end
    px[j] = xk[j, nbound]
    py[j] = yk[j, nbound]
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/OmniPathPreview.svg"},
    "doc": {"behavior": false}
  }
}
end


Test Cases ​

No test cases defined.

  • Examples

  • Experiments

  • Analyses