Dyad

Appearance

LIBRARY
Examples.OmniRobotPathModel.md

Examples.OmniRobotPathModel ​

Prediction model of ACADOSMPCOmniPathFollowingExample: OmniRobotPlant augmented with the progress    along the planned path and with the two components of the deviation from the path as outputs. It is never simulated; it is what the controller predicts with.

The progress is a state of the controller, not of the robot, and the rate at which it advances is a fifth control:

Missing open brace for subscript

so that     runs along the path at the nominal speed and the controller pays    for departing from it. Nothing ties the progress to the clock, which is what makes this a path-following problem rather than a tracking one: the planner's path is a curve, and how fast to travel along it is left to the controller.

The deviation is

with      the same line-arc path OmniPathPreview samples, evaluated here as a sum over the segments of what each contributes, clipped to the progress reached in it (see that component for the formula and omni_path_pose for the Julia counterpart). Clipping the arc length spent in the last segment also holds the path at its end once the progress runs past it, so the robot stops there. Both deviations are outputs of the model and are driven to zero by the cost, which makes the cost a nonlinear least squares on – the outputs are not linear in the state, since the path is not.

The three per-segment tables are online parameters rather than constants of the model, because that is what a path is in practice: the planner replans as the robot moves, and a new plan is a new value of the controller's p input and nothing else. Only the curvature, the length and the radius of each segment are given; the arc length and the heading at which each segment starts follow from them by accumulation, and seg_straight, which selects between the straight and the arc form, is     – one for a straight segment, where both are zero, and zero for an arc.

This component extends from MPCComponents.Examples.OmniRobotPlant

Usage ​

MPCComponents.Examples.OmniRobotPathModel(m=5.38, a_max=1.0, body_length=0.332, body_width=0.325, body_height=0.1335, lx=0.085, ly=0.135, wheel_radius=0.05, wheel_width=0.045, L=lx + ly, J=m * (body_length * body_length + body_width * body_width) / 12, f_max=m * a_max / 4, b=1.0, b_om=0.05, px0=0.0, py0=0.0, th0=0.0, vx0=0.0, vy0=0.0, om0=0.0, v_nom=0.4, s0=0.0, seg_curvature=omni_path_curvatures(), seg_length=omni_path_lengths(), seg_radius=omni_path_radii())

Parameters: ​

NameDescriptionUnitsDefault value
nsegNumber of segments of the planned path (derived)–omni_path_segment_count()
nboundNumber of segment boundaries of the planned path (derived)–nseg + 1
mMass of the platform–5.38
a_maxAcceleration limit of the platform–1.0
body_lengthLength of the chassis–0.332
body_widthWidth of the chassis–0.325
body_heightHeight of the chassis–0.1335
lxLongitudinal offset of each wheel from the centre of the platform–0.085
lyLateral offset of each wheel from the centre of the platform–0.135
wheel_radiusRadius of the mecanum wheels–0.05
wheel_widthWidth of the mecanum wheels–0.045
bViscous rolling resistance–1.0
b_omViscous yaw damping–0.05
px0Initial position along the corridor–0.0
py0Initial position across the corridor–0.0
th0Initial heading–0.0
vx0Initial forward velocity–0.0
vy0Initial lateral velocity–0.0
om0Initial yaw rate–0.0
v_nomRate of progress along the path at which the deviation control is free–0.4
s0Progress along the path at the start–0.0
seg_curvatureCurvature of each segment of the planned path; an online parameter, so the planner may replan–omni_path_curvatures()
seg_lengthLength of each segment of the planned path; an online parameter–omni_path_lengths()
seg_radiusRadius of each segment of the planned path, zero for a straight one; an online parameter–omni_path_radii()

Connectors ​

  • f1 - This connector represents a real signal as an input to a component (RealInput)

  • f2 - This connector represents a real signal as an input to a component (RealInput)

  • f3 - This connector represents a real signal as an input to a component (RealInput)

  • f4 - This connector represents a real signal as an input to a component (RealInput)

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

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

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

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

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

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

  • ds - This connector represents a real signal as an input to a component (RealInput)

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

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

Variables ​

NameDescriptionUnits
pxPosition along the corridor–
pyPosition across the corridor–
thHeading–
vxForward velocity in the body frame–
vyLateral velocity in the body frame–
omYaw rate–
FxForward force in the body frame–
FyLateral force in the body frame–
tauYaw torque–
w1Rolling speed of the front-left wheel–
w2Rolling speed of the front-right wheel–
w3Rolling speed of the rear-left wheel–
w4Rolling speed of the rear-right wheel–
seg_straightOne for a straight segment, zero for an arc (derived from the curvature and the radius)–
seg_startArc length at which each segment starts (derived from the lengths)–
seg_headingHeading at which each segment starts (derived from the curvatures and the lengths)–
sProgress along the planned path–
dArc length spent in each segment–
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
"""
Prediction model of `ACADOSMPCOmniPathFollowingExample`: `OmniRobotPlant` augmented with the progress
``\\sigma`` along the planned path and with the two components of the deviation from the path as
outputs. It is never simulated; it is what the controller predicts with.

The progress is a state of the controller, not of the robot, and the rate at which it advances is a
fifth control:

```math
\\dot\\sigma = v_\\mathrm{nom} + \\Delta v
```

so that ``\\Delta v = 0`` runs along the path at the nominal speed and the controller pays
``Q_2[5,5] \\Delta v^2`` for departing from it. Nothing ties the progress to the clock, which is what
makes this a path-following problem rather than a tracking one: the planner's path is a curve, and
how fast to travel along it is left to the controller.

The deviation is

```math
e_x = p_x - x(\\sigma), \\qquad e_y = p_y - y(\\sigma)
```

with ``(x(\\sigma), y(\\sigma))`` the same line-arc path `OmniPathPreview` samples, evaluated here as a
sum over the segments of what each contributes, clipped to the progress reached in it (see that
component for the formula and `omni_path_pose` for the Julia counterpart). Clipping the arc length
spent in the last segment also holds the path at its end once the progress runs past it, so the robot
stops there. Both deviations are outputs of the model and are driven to zero by the cost, which makes
the cost a nonlinear least squares on ``y(x)`` -- the outputs are not linear in the state, since the
path is not.

The three per-segment tables are online parameters rather than constants of the model, because that is
what a path is in practice: the planner replans as the robot moves, and a new plan is a new value of
the controller's `p` input and nothing else. Only the curvature, the length and the radius of each
segment are given; the arc length and the heading at which each segment starts follow from them by
accumulation, and `seg_straight`, which selects between the straight and the arc form, is
``1 - \\kappa_k R_k`` -- one for a straight segment, where both are zero, and zero for an arc.
"""
component OmniRobotPathModel
  extends MPCComponents.Examples.OmniRobotPlant
  "Correction to the nominal rate of progress along the path"
  ds = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 900, "y2": 1000}}}
  }
  "Deviation from the path along the corridor"
  ex = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 900, "y2": 1000}}}
  }
  "Deviation from the path across the corridor"
  ey = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 1020, "y2": 1120}}}
  }
  "Rate of progress along the path at which the deviation control is free"
  parameter v_nom::Real = 0.4
  "Progress along the path at the start"
  parameter s0::Real = 0.0
  "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
  "Curvature of each segment of the planned path; an online parameter, so the planner may replan"
  parameter seg_curvature::Real[nseg] = omni_path_curvatures()
  "Length of each segment of the planned path; an online parameter"
  parameter seg_length::Real[nseg] = omni_path_lengths()
  "Radius of each segment of the planned path, zero for a straight one; an online parameter"
  parameter seg_radius::Real[nseg] = omni_path_radii()
  "One for a straight segment, zero for an arc (derived from the curvature and the radius)"
  variable seg_straight::Real[nseg]
  "Arc length at which each segment starts (derived from the lengths)"
  variable seg_start::Real[nbound]
  "Heading at which each segment starts (derived from the curvatures and the lengths)"
  variable seg_heading::Real[nbound]
  "Progress along the planned path"
  variable s::Real
  "Arc length spent in each segment"
  variable d::Real[nseg]
  "Coordinate along the corridor after the first k segments"
  variable xk::Real[nbound]
  "Coordinate across the corridor after the first k segments"
  variable yk::Real[nbound]
relations
  initial s = s0
  der(s) = v_nom + ds
  xk[1] = 0
  yk[1] = 0
  seg_start[1] = 0
  seg_heading[1] = 0
  for k in 1:nseg
    seg_straight[k] = 1 - seg_curvature[k] * seg_radius[k]
    seg_start[k + 1] = seg_start[k] + seg_length[k]
    seg_heading[k + 1] = seg_heading[k] + seg_curvature[k] * seg_length[k]
  end
  for k in 1:nseg
    d[k] = max(min(s - seg_start[k], seg_length[k]), 0)
    xk[k + 1] = xk[k] + seg_straight[k] * d[k] * cos(seg_heading[k]) + seg_radius[k] * (sin(seg_heading[k] + seg_curvature[k] * d[k]) - sin(seg_heading[k]))
    yk[k + 1] = yk[k] + seg_straight[k] * d[k] * sin(seg_heading[k]) + seg_radius[k] * (cos(seg_heading[k]) - cos(seg_heading[k] + seg_curvature[k] * d[k]))
  end
  ex = px - xk[nbound]
  ey = py - yk[nbound]
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/OmniRobotPlant.svg"},
    "doc": {"behavior": false}
  }
}
end
Flattened Source
dyad
"""
Prediction model of `ACADOSMPCOmniPathFollowingExample`: `OmniRobotPlant` augmented with the progress
``\\sigma`` along the planned path and with the two components of the deviation from the path as
outputs. It is never simulated; it is what the controller predicts with.

The progress is a state of the controller, not of the robot, and the rate at which it advances is a
fifth control:

```math
\\dot\\sigma = v_\\mathrm{nom} + \\Delta v
```

so that ``\\Delta v = 0`` runs along the path at the nominal speed and the controller pays
``Q_2[5,5] \\Delta v^2`` for departing from it. Nothing ties the progress to the clock, which is what
makes this a path-following problem rather than a tracking one: the planner's path is a curve, and
how fast to travel along it is left to the controller.

The deviation is

```math
e_x = p_x - x(\\sigma), \\qquad e_y = p_y - y(\\sigma)
```

with ``(x(\\sigma), y(\\sigma))`` the same line-arc path `OmniPathPreview` samples, evaluated here as a
sum over the segments of what each contributes, clipped to the progress reached in it (see that
component for the formula and `omni_path_pose` for the Julia counterpart). Clipping the arc length
spent in the last segment also holds the path at its end once the progress runs past it, so the robot
stops there. Both deviations are outputs of the model and are driven to zero by the cost, which makes
the cost a nonlinear least squares on ``y(x)`` -- the outputs are not linear in the state, since the
path is not.

The three per-segment tables are online parameters rather than constants of the model, because that is
what a path is in practice: the planner replans as the robot moves, and a new plan is a new value of
the controller's `p` input and nothing else. Only the curvature, the length and the radius of each
segment are given; the arc length and the heading at which each segment starts follow from them by
accumulation, and `seg_straight`, which selects between the straight and the arc form, is
``1 - \\kappa_k R_k`` -- one for a straight segment, where both are zero, and zero for an arc.
"""
component OmniRobotPathModel
  "Drive force of the front-left wheel"
  f1 = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 100, "y2": 200}}}
  }
  "Drive force of the front-right wheel"
  f2 = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 330, "y2": 430}}}
  }
  "Drive force of the rear-left wheel"
  f3 = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 560, "y2": 660}}}
  }
  "Drive force of the rear-right wheel"
  f4 = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 790, "y2": 890}}}
  }
  "Position along the corridor"
  px_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 50, "y2": 150}}}
  }
  "Position across the corridor"
  py_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 200, "y2": 300}}}
  }
  "Heading"
  th_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 350, "y2": 450}}}
  }
  "Forward velocity in the body frame"
  vx_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 500, "y2": 600}}}
  }
  "Lateral velocity in the body frame"
  vy_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 650, "y2": 750}}}
  }
  "Yaw rate"
  om_out = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 800, "y2": 900}}}
  }
  "Mass of the platform"
  parameter m::Real = 5.38
  "Acceleration limit of the platform"
  parameter a_max::Real = 1.0
  "Length of the chassis"
  parameter body_length::Real = 0.332
  "Width of the chassis"
  parameter body_width::Real = 0.325
  "Height of the chassis"
  parameter body_height::Real = 0.1335
  "Longitudinal offset of each wheel from the centre of the platform"
  parameter lx::Real = 0.085
  "Lateral offset of each wheel from the centre of the platform"
  parameter ly::Real = 0.135
  "Radius of the mecanum wheels"
  parameter wheel_radius::Real = 0.05
  "Width of the mecanum wheels"
  parameter wheel_width::Real = 0.045
  "Lever arm of the wheels, the sum of their two offsets (derived)"
  final parameter L::Real = lx + ly
  "Yaw inertia of the platform, its chassis taken as a uniform plate (derived)"
  final parameter J::Real = m * (body_length * body_length + body_width * body_width) / 12
  "Force bound of one wheel, the acceleration limit of the platform spread over its four wheels (derived)"
  final parameter f_max::Real = m * a_max / 4
  "Viscous rolling resistance"
  parameter b::Real = 1.0
  "Viscous yaw damping"
  parameter b_om::Real = 0.05
  "Initial position along the corridor"
  parameter px0::Real = 0.0
  "Initial position across the corridor"
  parameter py0::Real = 0.0
  "Initial heading"
  parameter th0::Real = 0.0
  "Initial forward velocity"
  parameter vx0::Real = 0.0
  "Initial lateral velocity"
  parameter vy0::Real = 0.0
  "Initial yaw rate"
  parameter om0::Real = 0.0
  "Position along the corridor"
  variable px::Real
  "Position across the corridor"
  variable py::Real
  "Heading"
  variable th::Real
  "Forward velocity in the body frame"
  variable vx::Real
  "Lateral velocity in the body frame"
  variable vy::Real
  "Yaw rate"
  variable om::Real
  "Forward force in the body frame"
  variable Fx::Real
  "Lateral force in the body frame"
  variable Fy::Real
  "Yaw torque"
  variable tau::Real
  "Rolling speed of the front-left wheel"
  variable w1::Real
  "Rolling speed of the front-right wheel"
  variable w2::Real
  "Rolling speed of the rear-left wheel"
  variable w3::Real
  "Rolling speed of the rear-right wheel"
  variable w4::Real
  "Correction to the nominal rate of progress along the path"
  ds = RealInput() {
    "Dyad": {"placement": {"diagram": {"x1": -100, "x2": 0, "y1": 900, "y2": 1000}}}
  }
  "Deviation from the path along the corridor"
  ex = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 900, "y2": 1000}}}
  }
  "Deviation from the path across the corridor"
  ey = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "x2": 1100, "y1": 1020, "y2": 1120}}}
  }
  "Rate of progress along the path at which the deviation control is free"
  parameter v_nom::Real = 0.4
  "Progress along the path at the start"
  parameter s0::Real = 0.0
  "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
  "Curvature of each segment of the planned path; an online parameter, so the planner may replan"
  parameter seg_curvature::Real[nseg] = omni_path_curvatures()
  "Length of each segment of the planned path; an online parameter"
  parameter seg_length::Real[nseg] = omni_path_lengths()
  "Radius of each segment of the planned path, zero for a straight one; an online parameter"
  parameter seg_radius::Real[nseg] = omni_path_radii()
  "One for a straight segment, zero for an arc (derived from the curvature and the radius)"
  variable seg_straight::Real[nseg]
  "Arc length at which each segment starts (derived from the lengths)"
  variable seg_start::Real[nbound]
  "Heading at which each segment starts (derived from the curvatures and the lengths)"
  variable seg_heading::Real[nbound]
  "Progress along the planned path"
  variable s::Real
  "Arc length spent in each segment"
  variable d::Real[nseg]
  "Coordinate along the corridor after the first k segments"
  variable xk::Real[nbound]
  "Coordinate across the corridor after the first k segments"
  variable yk::Real[nbound]
relations
  initial px = px0
  initial py = py0
  initial th = th0
  initial vx = vx0
  initial vy = vy0
  initial om = om0
  Fx = f1 + f2 + f3 + f4
  Fy = -f1 + f2 + f3 - f4
  tau = L * (-f1 + f2 - f3 + f4)
  w1 = vx - vy - L * om
  w2 = vx + vy + L * om
  w3 = vx + vy - L * om
  w4 = vx - vy + L * om
  der(px) = vx * cos(th) - vy * sin(th)
  der(py) = vx * sin(th) + vy * cos(th)
  der(th) = om
  m * der(vx) = Fx + m * vy * om - b * vx
  m * der(vy) = Fy - m * vx * om - b * vy
  J * der(om) = tau - b_om * om
  px_out = px
  py_out = py
  th_out = th
  vx_out = vx
  vy_out = vy
  om_out = om
  initial s = s0
  der(s) = v_nom + ds
  xk[1] = 0
  yk[1] = 0
  seg_start[1] = 0
  seg_heading[1] = 0
  for k in 1:nseg
    seg_straight[k] = 1 - seg_curvature[k] * seg_radius[k]
    seg_start[k + 1] = seg_start[k] + seg_length[k]
    seg_heading[k + 1] = seg_heading[k] + seg_curvature[k] * seg_length[k]
  end
  for k in 1:nseg
    d[k] = max(min(s - seg_start[k], seg_length[k]), 0)
    xk[k + 1] = xk[k] + seg_straight[k] * d[k] * cos(seg_heading[k]) + seg_radius[k] * (sin(seg_heading[k] + seg_curvature[k] * d[k]) - sin(seg_heading[k]))
    yk[k + 1] = yk[k] + seg_straight[k] * d[k] * sin(seg_heading[k]) + seg_radius[k] * (cos(seg_heading[k]) - cos(seg_heading[k] + seg_curvature[k] * d[k]))
  end
  ex = px - xk[nbound]
  ey = py - yk[nbound]
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/OmniRobotPlant.svg"},
    "doc": {"behavior": false}
  }
}
end


Test Cases ​

No test cases defined.

  • Examples

  • Experiments

  • Analyses