LIBRARY

PlanarMechanics.examples.excavator.DigReference

Piston position references for the three excavator cylinders over one dig cycle.

The cycle visits five waypoints: start pose above the ground, penetration of the soil ahead of the machine, dragging the bucket through the soil towards the machine, curling the bucket, and lifting it out. Consecutive waypoints are interpolated with a smoothstep blend, giving continuously differentiable references with zero velocity at each waypoint.

Usage

MultibodyComponents.PlanarMechanics.examples.excavator.DigReference(t_wp=[0, 2.5, 6.5, 9, 12], wp_boom=[0.490, 0.061, 0.147, 0.147, 0.596], wp_stick=[0.154, 0.263, 0.100, 0.100, 0.154], wp_bucket=[0.360, 0.298, 0.304, 0.044, 0.044])

Parameters:

Name	Description	Units	Default value
t_wp	Waypoint times	s	[0, 2.5, 6.5, 9, 12]
wp_boom	Boom cylinder piston position at each waypoint	–	[0.490, 0.0...147, 0.596]
wp_stick	Stick cylinder piston position at each waypoint	–	[0.154, 0.2...100, 0.154]
wp_bucket	Bucket cylinder piston position at each waypoint	–	[0.360, 0.2...044, 0.044]

Connectors

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

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

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

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

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

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

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

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

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

Variables

Name	Description	Units
tau_seg	Normalized progress through each segment	–
blend	Smoothstep blend of each segment	–
dblend	Time derivative of the smoothstep blend of each segment	–
ddblend	Second time derivative of the smoothstep blend of each segment	–

Behavior

Source

"""
Piston position references for the three excavator cylinders over one dig
cycle.

The cycle visits five waypoints: start pose above the ground, penetration of
the soil ahead of the machine, dragging the bucket through the soil towards
the machine, curling the bucket, and lifting it out. Consecutive waypoints are
interpolated with a smoothstep blend, giving continuously differentiable
references with zero velocity at each waypoint.
"""
component DigReference
  "Boom cylinder piston position reference"
  s_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 20, "x2": 1050, "y2": 120}}}
  }
  "Stick cylinder piston position reference"
  s_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 320, "x2": 1050, "y2": 420}}}
  }
  "Bucket cylinder piston position reference"
  s_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 640, "x2": 1050, "y2": 740}}}
  }
  "Boom cylinder piston velocity reference"
  v_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 150, "x2": 1050, "y2": 250}}}
  }
  "Stick cylinder piston velocity reference"
  v_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 470, "x2": 1050, "y2": 570}}}
  }
  "Bucket cylinder piston velocity reference"
  v_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 800, "x2": 1050, "y2": 900}}}
  }
  "Boom cylinder piston acceleration reference"
  a_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 240, "x2": 1050, "y2": 300}}}
  }
  "Stick cylinder piston acceleration reference"
  a_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 570, "x2": 1050, "y2": 630}}}
  }
  "Bucket cylinder piston acceleration reference"
  a_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 900, "x2": 1050, "y2": 960}}}
  }
  "Waypoint times"
  parameter t_wp::Time[5] = [0, 2.5, 6.5, 9, 12]
  "Boom cylinder piston position at each waypoint"
  parameter wp_boom::Real[5] = [0.490, 0.061, 0.147, 0.147, 0.596]
  "Stick cylinder piston position at each waypoint"
  parameter wp_stick::Real[5] = [0.154, 0.263, 0.100, 0.100, 0.154]
  "Bucket cylinder piston position at each waypoint"
  parameter wp_bucket::Real[5] = [0.360, 0.298, 0.304, 0.044, 0.044]
  "Normalized progress through each segment"
  variable tau_seg::Real[4]
  "Smoothstep blend of each segment"
  variable blend::Real[4]
  "Time derivative of the smoothstep blend of each segment"
  variable dblend::Real[4]
  "Second time derivative of the smoothstep blend of each segment"
  variable ddblend::Real[4]
relations
  for i in 1:4
    tau_seg[i] = min(max((time - t_wp[i]) / (t_wp[i + 1] - t_wp[i]), 0), 1)
    blend[i] = tau_seg[i] ^ 2 * (3 - 2 * tau_seg[i])
    dblend[i] = 6 * tau_seg[i] * (1 - tau_seg[i]) / (t_wp[i + 1] - t_wp[i])
    # The interior expression is nonzero at the segment ends, so gate it to
    # the active segment
    ddblend[i] = ifelse(time < t_wp[i], 0, ifelse(time < t_wp[i + 1], (6 - 12 * tau_seg[i]) / (t_wp[i + 1] - t_wp[i]) ^ 2, 0))
  end
  s_boom = wp_boom[1] + (wp_boom[2] - wp_boom[1]) * blend[1] + (wp_boom[3] - wp_boom[2]) * blend[2] + (wp_boom[4] - wp_boom[3]) * blend[3] + (wp_boom[5] - wp_boom[4]) * blend[4]
  s_stick = wp_stick[1] + (wp_stick[2] - wp_stick[1]) * blend[1] + (wp_stick[3] - wp_stick[2]) * blend[2] + (wp_stick[4] - wp_stick[3]) * blend[3] + (wp_stick[5] - wp_stick[4]) * blend[4]
  s_bucket = wp_bucket[1] + (wp_bucket[2] - wp_bucket[1]) * blend[1] + (wp_bucket[3] - wp_bucket[2]) * blend[2] + (wp_bucket[4] - wp_bucket[3]) * blend[3] + (wp_bucket[5] - wp_bucket[4]) * blend[4]
  v_boom = (wp_boom[2] - wp_boom[1]) * dblend[1] + (wp_boom[3] - wp_boom[2]) * dblend[2] + (wp_boom[4] - wp_boom[3]) * dblend[3] + (wp_boom[5] - wp_boom[4]) * dblend[4]
  v_stick = (wp_stick[2] - wp_stick[1]) * dblend[1] + (wp_stick[3] - wp_stick[2]) * dblend[2] + (wp_stick[4] - wp_stick[3]) * dblend[3] + (wp_stick[5] - wp_stick[4]) * dblend[4]
  v_bucket = (wp_bucket[2] - wp_bucket[1]) * dblend[1] + (wp_bucket[3] - wp_bucket[2]) * dblend[2] + (wp_bucket[4] - wp_bucket[3]) * dblend[3] + (wp_bucket[5] - wp_bucket[4]) * dblend[4]
  a_boom = (wp_boom[2] - wp_boom[1]) * ddblend[1] + (wp_boom[3] - wp_boom[2]) * ddblend[2] + (wp_boom[4] - wp_boom[3]) * ddblend[3] + (wp_boom[5] - wp_boom[4]) * ddblend[4]
  a_stick = (wp_stick[2] - wp_stick[1]) * ddblend[1] + (wp_stick[3] - wp_stick[2]) * ddblend[2] + (wp_stick[4] - wp_stick[3]) * ddblend[3] + (wp_stick[5] - wp_stick[4]) * ddblend[4]
  a_bucket = (wp_bucket[2] - wp_bucket[1]) * ddblend[1] + (wp_bucket[3] - wp_bucket[2]) * ddblend[2] + (wp_bucket[4] - wp_bucket[3]) * ddblend[3] + (wp_bucket[5] - wp_bucket[4]) * ddblend[4]
metadata {"Dyad": {"icons": {"default": "dyad://MultibodyComponents/DigReference.svg"}}}
end

Flattened Source

"""
Piston position references for the three excavator cylinders over one dig
cycle.

The cycle visits five waypoints: start pose above the ground, penetration of
the soil ahead of the machine, dragging the bucket through the soil towards
the machine, curling the bucket, and lifting it out. Consecutive waypoints are
interpolated with a smoothstep blend, giving continuously differentiable
references with zero velocity at each waypoint.
"""
component DigReference
  "Boom cylinder piston position reference"
  s_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 20, "x2": 1050, "y2": 120}}}
  }
  "Stick cylinder piston position reference"
  s_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 320, "x2": 1050, "y2": 420}}}
  }
  "Bucket cylinder piston position reference"
  s_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 640, "x2": 1050, "y2": 740}}}
  }
  "Boom cylinder piston velocity reference"
  v_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 150, "x2": 1050, "y2": 250}}}
  }
  "Stick cylinder piston velocity reference"
  v_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 470, "x2": 1050, "y2": 570}}}
  }
  "Bucket cylinder piston velocity reference"
  v_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 800, "x2": 1050, "y2": 900}}}
  }
  "Boom cylinder piston acceleration reference"
  a_boom = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 240, "x2": 1050, "y2": 300}}}
  }
  "Stick cylinder piston acceleration reference"
  a_stick = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 570, "x2": 1050, "y2": 630}}}
  }
  "Bucket cylinder piston acceleration reference"
  a_bucket = RealOutput() {
    "Dyad": {"placement": {"diagram": {"x1": 950, "y1": 900, "x2": 1050, "y2": 960}}}
  }
  "Waypoint times"
  parameter t_wp::Time[5] = [0, 2.5, 6.5, 9, 12]
  "Boom cylinder piston position at each waypoint"
  parameter wp_boom::Real[5] = [0.490, 0.061, 0.147, 0.147, 0.596]
  "Stick cylinder piston position at each waypoint"
  parameter wp_stick::Real[5] = [0.154, 0.263, 0.100, 0.100, 0.154]
  "Bucket cylinder piston position at each waypoint"
  parameter wp_bucket::Real[5] = [0.360, 0.298, 0.304, 0.044, 0.044]
  "Normalized progress through each segment"
  variable tau_seg::Real[4]
  "Smoothstep blend of each segment"
  variable blend::Real[4]
  "Time derivative of the smoothstep blend of each segment"
  variable dblend::Real[4]
  "Second time derivative of the smoothstep blend of each segment"
  variable ddblend::Real[4]
relations
  for i in 1:4
    tau_seg[i] = min(max((time - t_wp[i]) / (t_wp[i + 1] - t_wp[i]), 0), 1)
    blend[i] = tau_seg[i] ^ 2 * (3 - 2 * tau_seg[i])
    dblend[i] = 6 * tau_seg[i] * (1 - tau_seg[i]) / (t_wp[i + 1] - t_wp[i])
    # The interior expression is nonzero at the segment ends, so gate it to
    # the active segment
    ddblend[i] = ifelse(time < t_wp[i], 0, ifelse(time < t_wp[i + 1], (6 - 12 * tau_seg[i]) / (t_wp[i + 1] - t_wp[i]) ^ 2, 0))
  end
  s_boom = wp_boom[1] + (wp_boom[2] - wp_boom[1]) * blend[1] + (wp_boom[3] - wp_boom[2]) * blend[2] + (wp_boom[4] - wp_boom[3]) * blend[3] + (wp_boom[5] - wp_boom[4]) * blend[4]
  s_stick = wp_stick[1] + (wp_stick[2] - wp_stick[1]) * blend[1] + (wp_stick[3] - wp_stick[2]) * blend[2] + (wp_stick[4] - wp_stick[3]) * blend[3] + (wp_stick[5] - wp_stick[4]) * blend[4]
  s_bucket = wp_bucket[1] + (wp_bucket[2] - wp_bucket[1]) * blend[1] + (wp_bucket[3] - wp_bucket[2]) * blend[2] + (wp_bucket[4] - wp_bucket[3]) * blend[3] + (wp_bucket[5] - wp_bucket[4]) * blend[4]
  v_boom = (wp_boom[2] - wp_boom[1]) * dblend[1] + (wp_boom[3] - wp_boom[2]) * dblend[2] + (wp_boom[4] - wp_boom[3]) * dblend[3] + (wp_boom[5] - wp_boom[4]) * dblend[4]
  v_stick = (wp_stick[2] - wp_stick[1]) * dblend[1] + (wp_stick[3] - wp_stick[2]) * dblend[2] + (wp_stick[4] - wp_stick[3]) * dblend[3] + (wp_stick[5] - wp_stick[4]) * dblend[4]
  v_bucket = (wp_bucket[2] - wp_bucket[1]) * dblend[1] + (wp_bucket[3] - wp_bucket[2]) * dblend[2] + (wp_bucket[4] - wp_bucket[3]) * dblend[3] + (wp_bucket[5] - wp_bucket[4]) * dblend[4]
  a_boom = (wp_boom[2] - wp_boom[1]) * ddblend[1] + (wp_boom[3] - wp_boom[2]) * ddblend[2] + (wp_boom[4] - wp_boom[3]) * ddblend[3] + (wp_boom[5] - wp_boom[4]) * ddblend[4]
  a_stick = (wp_stick[2] - wp_stick[1]) * ddblend[1] + (wp_stick[3] - wp_stick[2]) * ddblend[2] + (wp_stick[4] - wp_stick[3]) * ddblend[3] + (wp_stick[5] - wp_stick[4]) * ddblend[4]
  a_bucket = (wp_bucket[2] - wp_bucket[1]) * ddblend[1] + (wp_bucket[3] - wp_bucket[2]) * ddblend[2] + (wp_bucket[4] - wp_bucket[3]) * ddblend[3] + (wp_bucket[5] - wp_bucket[4]) * ddblend[4]
metadata {"Dyad": {"icons": {"default": "dyad://MultibodyComponents/DigReference.svg"}}}
end

Test Cases

No test cases defined.

Related

• Examples

• Experiments

• Analyses