Dyad

Appearance

LIBRARY
examples.TrajectoryPlannersTest.md

examples.TrajectoryPlannersTest ​

Example model for the Kinematic5 trajectory planner. The velocity output is passed through a BlockComponents.Continuous.Integrator so that the integrated velocity can be compared against the position output.

Usage ​

MultibodyComponents.examples.TrajectoryPlannersTest()

Behavior ​

julia
using MultibodyComponents #hide
using ModelingToolkit #hide
@named sys = MultibodyComponents.examples.TrajectoryPlannersTest() #hide
full_equations(sys) #hide
<< @example-block not executed in draft mode >>

Source ​

dyad
"""
Example model for the Kinematic5 trajectory planner. The velocity output is
passed through a BlockComponents.Continuous.Integrator so that the integrated velocity can
be compared against the position output.
"""
example component TrajectoryPlannersTest
  "5th order polynomial trajectory planner"
  kin5 = MultibodyComponents.Kinematic5(nout = 1, tf = 2.0, q0 = [0.0], q1 = [1.0]) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 290, "x2": 250, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  "Integrator for the Kinematic5 velocity output"
  int_kin5 = BlockComponents.Continuous.Integrator(k = 1.0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 290, "x2": 580, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  kinematicptpboundedjerk = MultibodyComponents.KinematicPTPBoundedJerk() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 450, "x2": 250, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  int_bounded_jerk = BlockComponents.Continuous.Integrator() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 450, "x2": 580, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  selector1 = MultibodyComponents.Selector(nu = 1, index = 1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 290, "x2": 420, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  selector2 = MultibodyComponents.Selector(nu = 1, index = 1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 450, "x2": 420, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  kinematicptp = MultibodyComponents.KinematicPTP() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 610, "x2": 250, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  selector = MultibodyComponents.Selector() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 610, "x2": 420, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  int_ptp = BlockComponents.Continuous.Integrator() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 610, "x2": 580, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
relations
  connect(kin5.qd, selector1.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(selector1.y, int_kin5.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(selector2.y, int_bounded_jerk.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(kinematicptpboundedjerk.qd, selector2.u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 280, "y": 488}, {"x": 280, "y": 500}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(selector.y, int_ptp.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
  connect(kinematicptp.qd, selector.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
metadata {"Dyad": {"tests": {"case1": {"stop": 3}}}}
end
Flattened Source
dyad
"""
Example model for the Kinematic5 trajectory planner. The velocity output is
passed through a BlockComponents.Continuous.Integrator so that the integrated velocity can
be compared against the position output.
"""
example component TrajectoryPlannersTest
  "5th order polynomial trajectory planner"
  kin5 = MultibodyComponents.Kinematic5(nout = 1, tf = 2.0, q0 = [0.0], q1 = [1.0]) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 290, "x2": 250, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  "Integrator for the Kinematic5 velocity output"
  int_kin5 = BlockComponents.Continuous.Integrator(k = 1.0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 290, "x2": 580, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  kinematicptpboundedjerk = MultibodyComponents.KinematicPTPBoundedJerk() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 450, "x2": 250, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  int_bounded_jerk = BlockComponents.Continuous.Integrator() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 450, "x2": 580, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  selector1 = MultibodyComponents.Selector(nu = 1, index = 1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 290, "x2": 420, "y2": 390, "rot": 0}
      },
      "tags": []
    }
  }
  selector2 = MultibodyComponents.Selector(nu = 1, index = 1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 450, "x2": 420, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  kinematicptp = MultibodyComponents.KinematicPTP() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 610, "x2": 250, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  selector = MultibodyComponents.Selector() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 320, "y1": 610, "x2": 420, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  int_ptp = BlockComponents.Continuous.Integrator() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 480, "y1": 610, "x2": 580, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
relations
  connect(kin5.qd, selector1.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(selector1.y, int_kin5.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(selector2.y, int_bounded_jerk.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(kinematicptpboundedjerk.qd, selector2.u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 280, "y": 488}, {"x": 280, "y": 500}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(selector.y, int_ptp.u) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
  connect(kinematicptp.qd, selector.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
metadata {"Dyad": {"tests": {"case1": {"stop": 3}}}}
end


Test 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.examples.TrajectoryPlannersTest()
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 = 3e+0, abstol=1e-6, reltol=1e-6)
sol_case1 = rebuild_sol(result_case1)
<< @setup-block not executed in draft mode >>

  • Examples

  • Experiments

  • Analyses