From 4ead78ceb32175e16b3834746073b94847b6ca82 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 03:45:46 -0400 Subject: [PATCH 01/53] vibed CI benchmark bot --- .github/workflows/benchmark.yml | 173 ++++++++++++++++ .gitignore | 3 + Cargo.lock | 2 + scripts/bench-local.sh | 125 ++++++++++++ scripts/bench-report.mjs | 192 ++++++++++++++++++ scripts/render-traj.mjs | 108 ++++++++++ scripts/run-bench.sh | 37 ++++ src-cli/Cargo.toml | 2 + src-cli/src/main.rs | 153 ++++++++++---- .../full_field_sweeper.traj | 184 +++++++++++++++++ .../swerve-base-high-dt/project.chor | 110 ++++++++++ .../swerve-base-high-dt/small_curve.traj | 108 ++++++++++ .../swerve-base-high-dt/straight_line.traj | 102 ++++++++++ .../swerve-base-high-dt/sweep_auto.traj | 157 ++++++++++++++ .../swerve-base/full_field_sweeper.traj | 184 +++++++++++++++++ test-projects/swerve-base/project.chor | 110 ++++++++++ test-projects/swerve-base/small_curve.traj | 108 ++++++++++ test-projects/swerve-base/straight_line.traj | 102 ++++++++++ test-projects/swerve-base/sweep_auto.traj | 157 ++++++++++++++ .../swerve-fast/full_field_sweeper.traj | 184 +++++++++++++++++ test-projects/swerve-fast/project.chor | 110 ++++++++++ test-projects/swerve-fast/small_curve.traj | 108 ++++++++++ test-projects/swerve-fast/straight_line.traj | 102 ++++++++++ test-projects/swerve-fast/sweep_auto.traj | 157 ++++++++++++++ .../swerve-heavy/full_field_sweeper.traj | 184 +++++++++++++++++ test-projects/swerve-heavy/project.chor | 110 ++++++++++ test-projects/swerve-heavy/small_curve.traj | 108 ++++++++++ test-projects/swerve-heavy/straight_line.traj | 102 ++++++++++ test-projects/swerve-heavy/sweep_auto.traj | 157 ++++++++++++++ .../swerve-weak/full_field_sweeper.traj | 184 +++++++++++++++++ test-projects/swerve-weak/project.chor | 110 ++++++++++ test-projects/swerve-weak/small_curve.traj | 108 ++++++++++ test-projects/swerve-weak/straight_line.traj | 102 ++++++++++ test-projects/swerve-weak/sweep_auto.traj | 157 ++++++++++++++ 34 files changed, 4057 insertions(+), 43 deletions(-) create mode 100644 .github/workflows/benchmark.yml create mode 100755 scripts/bench-local.sh create mode 100644 scripts/bench-report.mjs create mode 100644 scripts/render-traj.mjs create mode 100755 scripts/run-bench.sh create mode 100644 test-projects/swerve-base-high-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-base-high-dt/project.chor create mode 100644 test-projects/swerve-base-high-dt/small_curve.traj create mode 100644 test-projects/swerve-base-high-dt/straight_line.traj create mode 100644 test-projects/swerve-base-high-dt/sweep_auto.traj create mode 100644 test-projects/swerve-base/full_field_sweeper.traj create mode 100755 test-projects/swerve-base/project.chor create mode 100644 test-projects/swerve-base/small_curve.traj create mode 100644 test-projects/swerve-base/straight_line.traj create mode 100644 test-projects/swerve-base/sweep_auto.traj create mode 100644 test-projects/swerve-fast/full_field_sweeper.traj create mode 100755 test-projects/swerve-fast/project.chor create mode 100644 test-projects/swerve-fast/small_curve.traj create mode 100644 test-projects/swerve-fast/straight_line.traj create mode 100644 test-projects/swerve-fast/sweep_auto.traj create mode 100644 test-projects/swerve-heavy/full_field_sweeper.traj create mode 100755 test-projects/swerve-heavy/project.chor create mode 100644 test-projects/swerve-heavy/small_curve.traj create mode 100644 test-projects/swerve-heavy/straight_line.traj create mode 100644 test-projects/swerve-heavy/sweep_auto.traj create mode 100644 test-projects/swerve-weak/full_field_sweeper.traj create mode 100755 test-projects/swerve-weak/project.chor create mode 100644 test-projects/swerve-weak/small_curve.traj create mode 100644 test-projects/swerve-weak/straight_line.traj create mode 100644 test-projects/swerve-weak/sweep_auto.traj diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml new file mode 100644 index 0000000000..cef47db948 --- /dev/null +++ b/.github/workflows/benchmark.yml @@ -0,0 +1,173 @@ +name: PR Benchmark + +# Comment-triggered: a maintainer (OWNER / MEMBER / COLLABORATOR) types +# `/bench` on a PR and this workflow builds the PR's choreo-cli and the base +# branch's choreo-cli, runs both against the PR's test-projects/, computes +# per-trajectory solve-time / length / duration deltas, renders each trajectory +# as an SVG with per-step linear-acceleration coloring, uploads everything as +# an artifact, and posts a sticky PR comment with the comparison table. + +on: + issue_comment: + types: [created] + +permissions: + pull-requests: write + contents: read + actions: read + +concurrency: + group: bench-${{ github.event.issue.number }} + cancel-in-progress: true + +jobs: + bench: + # Only run when: + # - the comment is on a PR (not a plain issue) + # - the body starts with "/bench" + # - the commenter has write access (gates external drive-by triggers) + if: >- + github.event.issue.pull_request && + startsWith(github.event.comment.body, '/bench') && + (github.event.comment.author_association == 'OWNER' || + github.event.comment.author_association == 'MEMBER' || + github.event.comment.author_association == 'COLLABORATOR') + runs-on: ubuntu-24.04 + steps: + - name: Acknowledge the trigger comment + uses: peter-evans/create-or-update-comment@v4 + with: + comment-id: ${{ github.event.comment.id }} + reactions: rocket + + - name: Resolve PR head + base + id: pr + uses: actions/github-script@v7 + with: + script: | + const pr = await github.rest.pulls.get({ + owner: context.repo.owner, + repo: context.repo.repo, + pull_number: context.issue.number, + }); + core.setOutput('head_sha', pr.data.head.sha); + core.setOutput('base_sha', pr.data.base.sha); + core.setOutput('head_repo', pr.data.head.repo.full_name); + + - name: Checkout PR head + uses: actions/checkout@v6 + with: + repository: ${{ steps.pr.outputs.head_repo }} + ref: ${{ steps.pr.outputs.head_sha }} + fetch-depth: 0 + persist-credentials: false + + - name: Install Linux dependencies + run: | + sudo apt-get update -q + sudo apt-get install -y \ + build-essential \ + curl \ + file \ + hyperfine \ + libayatana-appindicator3-dev \ + librsvg2-dev \ + libssl-dev \ + libwebkit2gtk-4.1-dev \ + libxdo-dev \ + wget + + - name: Set up sccache + uses: mozilla-actions/sccache-action@v0.0.10 + + - name: Build PR choreo-cli + run: cargo build --release -p choreo-cli + env: + RUSTC_WRAPPER: sccache + SCCACHE_GHA_ENABLED: "true" + + - name: Stash PR binary + run: cp target/release/choreo-cli /tmp/cli-pr + + - name: Snapshot PR test-projects for both runs + run: | + cp -r test-projects /tmp/tp-pr + cp -r test-projects /tmp/tp-base + + - name: Run PR benchmark + run: bash scripts/run-bench.sh /tmp/cli-pr /tmp/tp-pr /tmp/out-pr + + # Now overlay base-branch sources of just the Rust crates so we can + # build the base binary against PR's test-projects/ + scripts/. + - name: Fetch base ref + run: git fetch origin ${{ steps.pr.outputs.base_sha }} + + - name: Check out base sources (Rust crates only) + run: | + git checkout ${{ steps.pr.outputs.base_sha }} -- \ + src-core src-cli trajoptlib Cargo.lock Cargo.toml + + - name: Build base choreo-cli + run: cargo build --release -p choreo-cli + env: + RUSTC_WRAPPER: sccache + SCCACHE_GHA_ENABLED: "true" + + - name: Stash base binary + run: cp target/release/choreo-cli /tmp/cli-base + + - name: Restore PR sources for scripts + run: | + git checkout ${{ steps.pr.outputs.head_sha }} -- \ + src-core src-cli trajoptlib Cargo.lock Cargo.toml + + - name: Run base benchmark + run: bash scripts/run-bench.sh /tmp/cli-base /tmp/tp-base /tmp/out-base + continue-on-error: true # tolerate schema-incompatible base + + - name: Render trajectory SVGs (per-step accel coloring) + run: | + set -euo pipefail + render() { + local side=$1 + local root=$2 + for traj in "$root"/*/*.traj; do + variant=$(basename "$(dirname "$traj")") + name=$(basename "$traj" .traj) + out="${traj%.traj}.${side}.svg" + node scripts/render-traj.mjs --traj "$traj" --out "$out" \ + --title "${side^^} ${variant} · ${name}" || true + done + } + render pr /tmp/tp-pr + render base /tmp/tp-base + + - name: Build markdown report + env: + ARTIFACT_URL: ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }} + run: | + node scripts/bench-report.mjs \ + --pr /tmp/tp-pr --base /tmp/tp-base \ + --pr-reports /tmp/out-pr --base-reports /tmp/out-base \ + --artifact-url "$ARTIFACT_URL" \ + --out report.md + cat report.md + + - name: Upload bench artifact + if: always() + uses: actions/upload-artifact@v7 + with: + name: bench-output-${{ github.run_id }} + path: | + /tmp/out-pr/** + /tmp/out-base/** + /tmp/tp-pr/**/*.svg + /tmp/tp-base/**/*.svg + report.md + + - name: Post sticky comment on PR + uses: marocchino/sticky-pull-request-comment@v2 + with: + header: bench + number: ${{ github.event.issue.number }} + path: report.md diff --git a/.gitignore b/.gitignore index 241dc94a23..a5a7b7f5e8 100644 --- a/.gitignore +++ b/.gitignore @@ -54,3 +54,6 @@ CMakeUserPresets.json build/ cli/ test-tmp*/ + +# Local PR-benchmark output (scripts/bench-local.sh) +bench-local/ diff --git a/Cargo.lock b/Cargo.lock index 00794a83b2..3c6867e2d9 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -565,6 +565,8 @@ version = "2026.0.3" dependencies = [ "choreo-core", "clap", + "serde", + "serde_json", "tracing", "tracing-subscriber", ] diff --git a/scripts/bench-local.sh b/scripts/bench-local.sh new file mode 100755 index 0000000000..342e0e7fc1 --- /dev/null +++ b/scripts/bench-local.sh @@ -0,0 +1,125 @@ +#!/usr/bin/env bash +# Local equivalent of .github/workflows/benchmark.yml — runs the bench pipeline +# against your working tree, optionally comparing to a base git ref, and writes +# everything (SVGs + reports + markdown) into ./bench-local/. +# +# Usage: +# scripts/bench-local.sh # PR-only run, no comparison +# scripts/bench-local.sh # full comparison vs (e.g. main) +# +# Requires: cargo, node, hyperfine. The base build uses `git worktree` so your +# current checkout stays untouched. + +set -euo pipefail + +REPO_ROOT=$(git -C "$(dirname "$0")" rev-parse --show-toplevel) +cd "$REPO_ROOT" + +BASE_REF=${1:-} +OUT_ROOT="$REPO_ROOT/bench-local" +PR_TP="$OUT_ROOT/tp-pr" +BASE_TP="$OUT_ROOT/tp-base" +PR_OUT="$OUT_ROOT/out-pr" +BASE_OUT="$OUT_ROOT/out-base" +BASE_WORKTREE="$OUT_ROOT/base-worktree" + +if [ ! -d "$REPO_ROOT/test-projects" ]; then + echo "test-projects/ not found at repo root" >&2 + exit 1 +fi +for cmd in cargo node hyperfine; do + command -v "$cmd" >/dev/null || { echo "missing required command: $cmd" >&2; exit 1; } +done + +rm -rf "$OUT_ROOT" +mkdir -p "$PR_OUT" "$BASE_OUT" + +echo "==> Building PR choreo-cli (current tree)" +cargo build --release -p choreo-cli +PR_CLI="$REPO_ROOT/target/release/choreo-cli" +cp "$PR_CLI" "$OUT_ROOT/cli-pr" + +echo "==> Snapshotting test-projects/" +cp -r "$REPO_ROOT/test-projects" "$PR_TP" +if [ -n "$BASE_REF" ]; then + cp -r "$REPO_ROOT/test-projects" "$BASE_TP" +else + # No base ref — give the report builder an empty dir so the "base" columns + # collapse to "—" instead of silently comparing PR regen to whatever stale + # samples happen to be committed to test-projects/. + mkdir -p "$BASE_TP" +fi + +echo "==> Running PR benchmark" +bash "$REPO_ROOT/scripts/run-bench.sh" "$OUT_ROOT/cli-pr" "$PR_TP" "$PR_OUT" + +ARTIFACT_ARG=() +if [ -n "$BASE_REF" ]; then + if ! git rev-parse --verify --quiet "$BASE_REF" >/dev/null; then + echo "base ref '$BASE_REF' is not a valid git ref" >&2 + exit 1 + fi + echo "==> Setting up worktree for base ref '$BASE_REF'" + git worktree add --detach "$BASE_WORKTREE" "$BASE_REF" + trap 'git worktree remove --force "$BASE_WORKTREE" 2>/dev/null || true' EXIT + + # Build base CLI inside the base worktree using a separate target dir so we + # don't blow away the working-tree's incremental cache. + echo "==> Building base choreo-cli (in worktree, separate target dir)" + ( + cd "$BASE_WORKTREE" + CARGO_TARGET_DIR="$OUT_ROOT/base-target" cargo build --release -p choreo-cli + ) + cp "$OUT_ROOT/base-target/release/choreo-cli" "$OUT_ROOT/cli-base" + + echo "==> Running base benchmark" + bash "$REPO_ROOT/scripts/run-bench.sh" "$OUT_ROOT/cli-base" "$BASE_TP" "$BASE_OUT" || true +fi + +echo "==> Rendering SVGs" +render_side() { + local side=$1 + local root=$2 + for traj in "$root"/*/*.traj; do + [ -f "$traj" ] || continue + variant=$(basename "$(dirname "$traj")") + name=$(basename "$traj" .traj) + node "$REPO_ROOT/scripts/render-traj.mjs" \ + --traj "$traj" \ + --out "${traj%.traj}.${side}.svg" \ + --title "${side^^} ${variant} · ${name}" + done +} +render_side pr "$PR_TP" +if [ -n "$BASE_REF" ]; then + render_side base "$BASE_TP" +fi + +echo "==> Collecting SVGs into bench-local/svgs/" +mkdir -p "$OUT_ROOT/svgs" +find "$PR_TP" "$BASE_TP" -name "*.svg" -print0 2>/dev/null | while IFS= read -r -d '' f; do + variant=$(basename "$(dirname "$f")") + cp "$f" "$OUT_ROOT/svgs/${variant}__$(basename "$f")" +done + +echo "==> Building markdown report" +node "$REPO_ROOT/scripts/bench-report.mjs" \ + --pr "$PR_TP" \ + --base "$BASE_TP" \ + --pr-reports "$PR_OUT" \ + --base-reports "$BASE_OUT" \ + --artifact-url "file://$OUT_ROOT/svgs/" \ + --out "$OUT_ROOT/report.md" + +echo +echo "------------------------------------------------------------" +cat "$OUT_ROOT/report.md" +echo "------------------------------------------------------------" +echo +echo "Outputs:" +echo " Report: $OUT_ROOT/report.md" +echo " SVGs: $OUT_ROOT/svgs/" +echo " PR raw: $PR_OUT/ + $PR_TP/" +if [ -n "$BASE_REF" ]; then + echo " Base raw: $BASE_OUT/ + $BASE_TP/" +fi diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs new file mode 100644 index 0000000000..91d857c7a4 --- /dev/null +++ b/scripts/bench-report.mjs @@ -0,0 +1,192 @@ +#!/usr/bin/env node +// Build a markdown comparison report from two `run-bench.sh` outputs. +// +// Usage: +// node scripts/bench-report.mjs \ +// --pr --base \ +// --pr-reports --base-reports \ +// --artifact-url --out + +import { readFileSync, writeFileSync, existsSync, readdirSync, statSync } from "node:fs"; +import { join } from "node:path"; + +const argv = parseArgs(process.argv.slice(2)); +for (const k of ["pr", "base", "pr-reports", "base-reports", "out"]) { + if (!argv[k]) { + console.error(`missing required --${k}`); + process.exit(2); + } +} +const artifactUrl = argv["artifact-url"] ?? null; + +// Thresholds for highlighting regressions. +const SOLVE_PCT_THRESHOLD = 15; // bold/red if |pctSolve| > 15% +const LEN_DELTA_THRESHOLD = 0.01; // bold/red if |dLen| > 1 cm + +const variants = listVariants(argv.pr); +const rows = []; +const summary = { regressed: 0, improved: 0, base_failed: 0, total: 0 }; + +for (const variant of variants) { + const prReport = loadReport(argv["pr-reports"], variant); + const baseReport = loadReport(argv["base-reports"], variant); + const prDir = join(argv.pr, variant); + const baseDir = join(argv.base, variant); + + if (!prReport) { + rows.push(missingRow(variant, "(no PR report)")); + continue; + } + + for (const prEntry of prReport) { + const name = prEntry.name; + const baseEntry = baseReport?.find(r => r.name === name) ?? null; + const prTraj = trajMetrics(join(prDir, `${name}.traj`)); + const baseTraj = trajMetrics(join(baseDir, `${name}.traj`)); + + let status = "OK"; + if (!prEntry.ok) status = "PR_FAILED"; + else if (!baseEntry) status = "BASE_MISSING"; + else if (!baseEntry.ok) status = "BASE_FAILED"; + + const solvePr = prEntry.ok ? prEntry.solve_ms : null; + const solveBase = baseEntry?.ok ? baseEntry.solve_ms : null; + const dSolve = (solvePr != null && solveBase != null) ? solvePr - solveBase : null; + const pctSolve = (dSolve != null && solveBase > 0) ? (dSolve / solveBase) * 100 : null; + const dLen = (prTraj && baseTraj) ? prTraj.length_m - baseTraj.length_m : null; + const dTime = (prTraj && baseTraj) ? prTraj.total_s - baseTraj.total_s : null; + + const regressed = + status !== "OK" || + (pctSolve != null && Math.abs(pctSolve) > SOLVE_PCT_THRESHOLD) || + (dLen != null && Math.abs(dLen) > LEN_DELTA_THRESHOLD); + + summary.total++; + if (status === "BASE_MISSING" || status === "BASE_FAILED") summary.base_failed++; + else if (regressed && pctSolve != null && pctSolve > 0) summary.regressed++; + else if (pctSolve != null && pctSolve < -SOLVE_PCT_THRESHOLD) summary.improved++; + + rows.push({ variant, name, status, regressed, + solvePr, solveBase, dSolve, pctSolve, + lenPr: prTraj?.length_m, lenBase: baseTraj?.length_m, dLen, + timePr: prTraj?.total_s, timeBase: baseTraj?.total_s, dTime, + samplesPr: prTraj?.sample_count, samplesBase: baseTraj?.sample_count }); + } +} + +writeFileSync(argv.out, renderMarkdown(rows, summary, artifactUrl)); + +// ---------- helpers ---------- + +function listVariants(dir) { + if (!existsSync(dir)) return []; + return readdirSync(dir) + .filter(n => statSync(join(dir, n)).isDirectory()) + .filter(n => existsSync(join(dir, n, "project.chor"))) + .sort(); +} + +function loadReport(dir, variant) { + const p = join(dir, `${variant}.report.json`); + if (!existsSync(p)) return null; + try { return JSON.parse(readFileSync(p, "utf8")); } + catch { return null; } +} + +function trajMetrics(trajPath) { + if (!existsSync(trajPath)) return null; + let raw; + try { raw = JSON.parse(readFileSync(trajPath, "utf8")); } + catch { return null; } + const samples = raw?.trajectory?.samples ?? []; + if (samples.length < 2) return null; + let len = 0; + for (let i = 1; i < samples.length; i++) { + len += Math.hypot(samples[i].x - samples[i - 1].x, samples[i].y - samples[i - 1].y); + } + return { length_m: len, total_s: samples[samples.length - 1].t, sample_count: samples.length }; +} + +function missingRow(variant, note) { + return { variant, name: note, status: "NO_PR_REPORT", regressed: true }; +} + +function renderMarkdown(rows, summary, artifactUrl) { + const lines = []; + lines.push(`### Choreo trajectory benchmark`); + lines.push(""); + lines.push(`Compared ${summary.total} trajectories across ${new Set(rows.map(r => r.variant)).size} project variant(s).`); + const bits = []; + if (summary.regressed) bits.push(`**${summary.regressed} regressed** (solve time >${SOLVE_PCT_THRESHOLD}% slower or path length shifted >${LEN_DELTA_THRESHOLD}m)`); + if (summary.improved) bits.push(`${summary.improved} sped up`); + if (summary.base_failed) bits.push(`${summary.base_failed} unable to compare to base`); + lines.push(bits.length ? bits.join(" · ") : "No notable differences."); + lines.push(""); + lines.push("| Variant | Trajectory | Solve (PR) | Solve (base) | Δsolve | Length Δ | Time Δ | Samples (PR/base) | Status |"); + lines.push("|---|---|---:|---:|---:|---:|---:|:---:|:---:|"); + for (const r of rows) { + if (r.status === "NO_PR_REPORT") { + lines.push(`| \`${r.variant}\` | — | — | — | — | — | — | — | **${r.status}** |`); + continue; + } + lines.push("| " + [ + `\`${r.variant}\``, + `\`${r.name}\``, + fmtMs(r.solvePr), + fmtMs(r.solveBase), + fmtSolveDelta(r.dSolve, r.pctSolve, r.regressed), + fmtLenDelta(r.dLen, r.regressed), + fmtTimeDelta(r.dTime), + r.samplesPr != null && r.samplesBase != null ? `${r.samplesPr}/${r.samplesBase}` : (r.samplesPr ?? "—"), + r.status === "OK" ? "OK" : `**${r.status}**`, + ].join(" | ") + " |"); + } + lines.push(""); + if (artifactUrl) { + lines.push(`Trajectory renderings (per-step linear-acceleration coloring, matching the UI gradient) are uploaded as a workflow artifact — open the run page to download:`); + lines.push(""); + lines.push(`→ **[Download bench-output artifact](${artifactUrl})**`); + } + lines.push(""); + lines.push(`Solve times are wall-clock around \`generate()\` only (excluding CLI startup), measured with hyperfine \`--warmup 1 --runs 3\` and reported by the CLI's \`--report-json\`. \`Length Δ\` is the change in integrated path length (sum of segment lengths). \`Time Δ\` is the change in total trajectory duration. \`Samples\` shows the count from each side; very different counts mean point-wise comparisons would be misleading — use the artifact's SVGs for visual diff.`); + return lines.join("\n") + "\n"; +} + +function fmtMs(v) { + if (v == null) return "—"; + return `${v.toFixed(1)} ms`; +} + +function fmtSolveDelta(d, pct, regressed) { + if (d == null) return "—"; + const sign = d > 0 ? "+" : ""; + const txt = `${sign}${d.toFixed(1)} ms${pct != null ? ` (${sign}${pct.toFixed(1)}%)` : ""}`; + return regressed && pct != null && Math.abs(pct) > SOLVE_PCT_THRESHOLD ? `**${txt}**` : txt; +} + +function fmtLenDelta(d, regressed) { + if (d == null) return "—"; + const sign = d >= 0 ? "+" : ""; + const txt = `${sign}${d.toFixed(4)} m`; + return regressed && Math.abs(d) > LEN_DELTA_THRESHOLD ? `**${txt}**` : txt; +} + +function fmtTimeDelta(d) { + if (d == null) return "—"; + const sign = d >= 0 ? "+" : ""; + return `${sign}${d.toFixed(3)} s`; +} + +function parseArgs(args) { + const out = {}; + for (let i = 0; i < args.length; i++) { + const a = args[i]; + if (a.startsWith("--")) { + const key = a.slice(2); + const next = args[i + 1]; + if (next === undefined || next.startsWith("--")) { out[key] = true; } + else { out[key] = next; i++; } + } + } + return out; +} diff --git a/scripts/render-traj.mjs b/scripts/render-traj.mjs new file mode 100644 index 0000000000..9773393892 --- /dev/null +++ b/scripts/render-traj.mjs @@ -0,0 +1,108 @@ +#!/usr/bin/env node +// Render a Choreo .traj file as an SVG with per-segment color matching the +// UI's linear-acceleration gradient from +// src/components/config/robotconfig/PathGradient.tsx (linearAcceleration). +// +// Usage: +// node scripts/render-traj.mjs --traj --out +// [--field-svg ] [--title ] + +import { readFileSync, writeFileSync } from "node:fs"; +import { fileURLToPath } from "node:url"; +import { dirname, resolve } from "node:path"; + +const argv = parseArgs(process.argv.slice(2)); +if (!argv.traj || !argv.out) { + console.error("usage: render-traj.mjs --traj --out [--field-svg ] [--title ]"); + process.exit(2); +} + +// Field dimensions match src/components/field/svg/fields/FieldDimensions.tsx +const FIELD_W = 16.541; +const FIELD_H = 8.0692; +// Match FieldImage2026.svg's viewBox so embedded background lines up. +const VB_X = -0.5, VB_Y = -0.5, VB_W = 17.541, VB_H = 9.0692; + +const here = dirname(fileURLToPath(import.meta.url)); +const defaultField = resolve(here, "..", "src", "components", "field", "svg", "fields", "FieldImage2026.svg"); +const fieldPath = argv["field-svg"] ?? defaultField; +const fieldB64 = readFileSync(fieldPath).toString("base64"); + +const traj = JSON.parse(readFileSync(argv.traj, "utf8")).trajectory; +const samples = traj.samples ?? []; + +if (samples.length < 2) { + writeFileSync(argv.out, emptySvg(argv.title, fieldB64)); + process.exit(0); +} + +// Each segment runs from samples[i] -> samples[i+1] and is colored from the +// START sample's acceleration. Matches FieldGeneratedLines.tsx (i runs over +// segments, passed as `index: i` to the gradient function) and +// PathGradient.tsx:linearAcceleration (hsl(100*a/10, 100%, 50%) — NOT clamped; +// for accel > 10 m/s^2 the hue wraps past green into cyan/blue). +const segs = []; +for (let i = 0; i < samples.length - 1; i++) { + const s0 = samples[i]; + const s1 = samples[i + 1]; + const a = s0.vl !== undefined + ? Math.abs(s0.al + s0.ar) / 2 + : Math.hypot(s0.ax, s0.ay); + const hue = (100 * a) / 10; + segs.push( + `` + ); +} + +// The trajectory uses the same flip transform as FieldImage2026.svg's +// internal so we can write raw FRC field coords (origin at +// bottom-left) directly into elements. +const out = ` + + + ${segs.join("\n ")} + + ${titleElement(argv.title)} + +`; + +writeFileSync(argv.out, out); + +function titleElement(title) { + if (!title) return ""; + return `${escapeXml(title)}`; +} + +function emptySvg(title, fieldB64) { + return ` + + no samples${title ? ` — ${escapeXml(title)}` : ""} + +`; +} + +function fmt(n) { + return Number.isFinite(n) ? n.toFixed(4) : "0"; +} + +function escapeXml(s) { + return String(s).replace(/[<>&"]/g, c => ({ "<": "<", ">": ">", "&": "&", '"': """ }[c])); +} + +function parseArgs(args) { + const out = {}; + for (let i = 0; i < args.length; i++) { + const a = args[i]; + if (a.startsWith("--")) { + const key = a.slice(2); + const next = args[i + 1]; + if (next === undefined || next.startsWith("--")) { + out[key] = true; + } else { + out[key] = next; + i++; + } + } + } + return out; +} diff --git a/scripts/run-bench.sh b/scripts/run-bench.sh new file mode 100755 index 0000000000..5f18754e96 --- /dev/null +++ b/scripts/run-bench.sh @@ -0,0 +1,37 @@ +#!/usr/bin/env bash +# Run choreo-cli generation across every project variant in $2, using hyperfine +# for repeated-run statistics. The CLI's own --report-json carries the +# authoritative per-trajectory solve_ms (it brackets only generate(), not +# process startup). +# +# Usage: run-bench.sh +set -euo pipefail + +CLI=${1:?missing CLI binary path} +PROJECTS=${2:?missing test-projects dir} +OUT=${3:?missing output dir} + +mkdir -p "$OUT" + +shopt -s nullglob +project_dirs=("$PROJECTS"/*/) +if [ "${#project_dirs[@]}" -eq 0 ]; then + echo "no project subdirectories found under $PROJECTS" >&2 + exit 1 +fi + +for proj_dir in "${project_dirs[@]}"; do + name=$(basename "$proj_dir") + chor="$proj_dir/project.chor" + if [ ! -f "$chor" ]; then + echo "skipping $name (no project.chor)" >&2 + continue + fi + echo "::group::$name" + hyperfine \ + --warmup 1 \ + --runs 3 \ + --export-json "$OUT/$name.hyperfine.json" \ + "$CLI --chor $chor --all-trajectory --generate --report-json $OUT/$name.report.json" + echo "::endgroup::" +done diff --git a/src-cli/Cargo.toml b/src-cli/Cargo.toml index e83279c91c..8ee80bc24e 100644 --- a/src-cli/Cargo.toml +++ b/src-cli/Cargo.toml @@ -10,5 +10,7 @@ license = "BSD-3-Clause" [dependencies] choreo-core = { path = "../src-core" } clap = { version = "4.5.53", features = ["derive"] } +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" tracing = "0.1.44" tracing-subscriber = { version = "0.3.22", features = ["ansi", "fmt", "std", "json"] } diff --git a/src-cli/src/main.rs b/src-cli/src/main.rs index 831f7c8645..a225fdf564 100644 --- a/src-cli/src/main.rs +++ b/src-cli/src/main.rs @@ -5,6 +5,7 @@ use std::{ process::exit, sync::{Arc, Mutex}, thread::{self, JoinHandle}, + time::Instant, }; use choreo_core::{ @@ -15,6 +16,15 @@ use choreo_core::{ spec::trajectory::TrajectoryFile, }; use clap::Parser; +use serde::Serialize; + +#[derive(Serialize)] +struct TrajReport { + name: String, + solve_ms: f64, + ok: bool, + error: Option, +} const FORMATTING_OPTIONS: &str = "Formatting Options"; const FILE_OPTIONS: &str = "File Options"; @@ -26,6 +36,7 @@ enum CliAction { Generate { project_path: PathBuf, trajectory_names: Vec, + report_json: Option, }, Error(String), } @@ -79,6 +90,14 @@ pub struct Cli { required = true, )] pub generate: bool, + + #[arg( + long, + value_name = "path/to/report.json", + help_heading = FILE_OPTIONS, + help = "Write a JSON report with per-trajectory solve_ms timings to this path", + )] + pub report_json: Option, } impl Cli { @@ -104,6 +123,7 @@ impl Cli { return CliAction::Generate { project_path, trajectory_names: self.trajectory, + report_json: self.report_json, }; } CliAction::Error("Choreo file must be provided for generation.".to_string()) @@ -121,6 +141,7 @@ impl Cli { CliAction::Generate { project_path, trajectory_names, + report_json, } => { tracing::info!("CLIAction is Generate"); choreo_core::tokio::runtime::Builder::new_current_thread() @@ -131,6 +152,7 @@ impl Cli { resources, project_path, trajectory_names, + report_json, )); } CliAction::Error(e) => { @@ -145,6 +167,7 @@ impl Cli { resources: WritingResources, project_path: PathBuf, mut trajectory_names: Vec, + report_json: Option, ) { let deploy_root = project_path .parent() @@ -176,6 +199,7 @@ impl Cli { let mut thread_handles: Vec> = Vec::new(); let trajectories = Arc::new(Mutex::new(Vec::::new())); + let reports = Arc::new(Mutex::new(Vec::::new())); // generate trajectories for (i, trajectory_name) in trajectory_names.iter().enumerate() { @@ -186,6 +210,7 @@ impl Cli { ); let trajectories_window = Arc::clone(&trajectories); + let reports_window = Arc::clone(&reports); let trajectory = file_management::read_trajectory_file(&resources, trajectory_name.to_string()) .await @@ -194,51 +219,71 @@ impl Cli { let cln_project = project.clone(); let cln_resources = resources.clone(); let cln_trajectory_name = trajectory_name.clone(); - let handle = - thread::spawn( - move || match generate(cln_project.clone(), trajectory, i as i64) { - Ok(new_trajectory) => { - let runtime = - choreo_core::tokio::runtime::Builder::new_current_thread() - .enable_all() - .build() - .expect("Failed to build tokio runtime"); - let write_result = - runtime.block_on(file_management::write_trajectory_file( - &cln_resources, - &new_trajectory, - )); - match write_result { - Ok(_) => { - tracing::info!( - "Successfully generated trajectory {:} for {:}", - cln_trajectory_name, - cln_project.name - ); - } - Err(e) => { - tracing::error!( - "Failed to write trajectory {:} for {:}: {:}", - cln_trajectory_name, - cln_project.name, - e - ); - } + let handle = thread::spawn(move || { + let solve_start = Instant::now(); + let result = generate(cln_project.clone(), trajectory, i as i64); + let solve_ms = solve_start.elapsed().as_secs_f64() * 1000.0; + match result { + Ok(new_trajectory) => { + let runtime = choreo_core::tokio::runtime::Builder::new_current_thread() + .enable_all() + .build() + .expect("Failed to build tokio runtime"); + let write_result = + runtime.block_on(file_management::write_trajectory_file( + &cln_resources, + &new_trajectory, + )); + match write_result { + Ok(_) => { + tracing::info!( + "Successfully generated trajectory {:} for {:} in {:.1} ms", + cln_trajectory_name, + cln_project.name, + solve_ms + ); + } + Err(e) => { + tracing::error!( + "Failed to write trajectory {:} for {:}: {:}", + cln_trajectory_name, + cln_project.name, + e + ); } - trajectories_window - .lock() - .expect("Internal Choreo Thread has panicked; see stack trace.") - .push(new_trajectory); - } - Err(e) => { - tracing::error!( - "Failed to generate trajectory {:}: {:}", - cln_trajectory_name, - e - ); } - }, - ); + trajectories_window + .lock() + .expect("Internal Choreo Thread has panicked; see stack trace.") + .push(new_trajectory); + reports_window + .lock() + .expect("Internal Choreo Thread has panicked; see stack trace.") + .push(TrajReport { + name: cln_trajectory_name, + solve_ms, + ok: true, + error: None, + }); + } + Err(e) => { + tracing::error!( + "Failed to generate trajectory {:}: {:}", + cln_trajectory_name, + e + ); + reports_window + .lock() + .expect("Internal Choreo Thread has panicked; see stack trace.") + .push(TrajReport { + name: cln_trajectory_name, + solve_ms, + ok: false, + error: Some(e.to_string()), + }); + } + } + }); thread_handles.push(handle); } @@ -251,6 +296,28 @@ impl Cli { } } } + + if let Some(report_path) = report_json { + let reports_vec = reports + .lock() + .expect("Report mutex was poisoned by a panicked generation thread."); + match serde_json::to_string_pretty(&*reports_vec) { + Ok(json) => { + if let Err(e) = fs::write(&report_path, json) { + tracing::error!( + "Failed to write report JSON to {}: {}", + report_path.display(), + e + ); + } else { + tracing::info!("Wrote report JSON to {}", report_path.display()); + } + } + Err(e) => { + tracing::error!("Failed to serialize report JSON: {}", e); + } + } + } if let Some(codegen_root) = project.codegen.get_root() && let Some(deploy_root_str) = deploy_root.to_str() && let Some(pkg_name) = java::codegen_package_name(&project) diff --git a/test-projects/swerve-base-high-dt/full_field_sweeper.traj b/test-projects/swerve-base-high-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..d772fc87d1 --- /dev/null +++ b/test-projects/swerve-base-high-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-high-dt/project.chor b/test-projects/swerve-base-high-dt/project.chor new file mode 100755 index 0000000000..1fd8d31d68 --- /dev/null +++ b/test-projects/swerve-base-high-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2.0", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.02 N * m / A", + "val":0.02 + }, + "kV":{ + "exp":"0.00206896552 V / rpm", + "val":0.01975716537568162 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-base-high-dt/small_curve.traj b/test-projects/swerve-base-high-dt/small_curve.traj new file mode 100644 index 0000000000..9ffc61a53b --- /dev/null +++ b/test-projects/swerve-base-high-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-high-dt/straight_line.traj b/test-projects/swerve-base-high-dt/straight_line.traj new file mode 100644 index 0000000000..41e4360d5c --- /dev/null +++ b/test-projects/swerve-base-high-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-high-dt/sweep_auto.traj b/test-projects/swerve-base-high-dt/sweep_auto.traj new file mode 100644 index 0000000000..2942f1ac8b --- /dev/null +++ b/test-projects/swerve-base-high-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base/full_field_sweeper.traj b/test-projects/swerve-base/full_field_sweeper.traj new file mode 100644 index 0000000000..b21c0ce67b --- /dev/null +++ b/test-projects/swerve-base/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base/project.chor b/test-projects/swerve-base/project.chor new file mode 100755 index 0000000000..1fd8d31d68 --- /dev/null +++ b/test-projects/swerve-base/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2.0", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.02 N * m / A", + "val":0.02 + }, + "kV":{ + "exp":"0.00206896552 V / rpm", + "val":0.01975716537568162 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-base/small_curve.traj b/test-projects/swerve-base/small_curve.traj new file mode 100644 index 0000000000..d1c246e26d --- /dev/null +++ b/test-projects/swerve-base/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base/straight_line.traj b/test-projects/swerve-base/straight_line.traj new file mode 100644 index 0000000000..3e3a97ff66 --- /dev/null +++ b/test-projects/swerve-base/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base/sweep_auto.traj b/test-projects/swerve-base/sweep_auto.traj new file mode 100644 index 0000000000..60f3cc0689 --- /dev/null +++ b/test-projects/swerve-base/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast/full_field_sweeper.traj b/test-projects/swerve-fast/full_field_sweeper.traj new file mode 100644 index 0000000000..b21c0ce67b --- /dev/null +++ b/test-projects/swerve-fast/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast/project.chor b/test-projects/swerve-fast/project.chor new file mode 100755 index 0000000000..68fbd449ad --- /dev/null +++ b/test-projects/swerve-fast/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"5.2", + "val":5.2 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2.0", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.02 N * m / A", + "val":0.02 + }, + "kV":{ + "exp":"0.00206896552 V / rpm", + "val":0.01975716537568162 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-fast/small_curve.traj b/test-projects/swerve-fast/small_curve.traj new file mode 100644 index 0000000000..d1c246e26d --- /dev/null +++ b/test-projects/swerve-fast/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast/straight_line.traj b/test-projects/swerve-fast/straight_line.traj new file mode 100644 index 0000000000..3e3a97ff66 --- /dev/null +++ b/test-projects/swerve-fast/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast/sweep_auto.traj b/test-projects/swerve-fast/sweep_auto.traj new file mode 100644 index 0000000000..60f3cc0689 --- /dev/null +++ b/test-projects/swerve-fast/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy/full_field_sweeper.traj b/test-projects/swerve-heavy/full_field_sweeper.traj new file mode 100644 index 0000000000..b21c0ce67b --- /dev/null +++ b/test-projects/swerve-heavy/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy/project.chor b/test-projects/swerve-heavy/project.chor new file mode 100755 index 0000000000..b3277d63e0 --- /dev/null +++ b/test-projects/swerve-heavy/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2.0", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.02 N * m / A", + "val":0.02 + }, + "kV":{ + "exp":"0.00206896552 V / rpm", + "val":0.01975716537568162 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-heavy/small_curve.traj b/test-projects/swerve-heavy/small_curve.traj new file mode 100644 index 0000000000..d1c246e26d --- /dev/null +++ b/test-projects/swerve-heavy/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy/straight_line.traj b/test-projects/swerve-heavy/straight_line.traj new file mode 100644 index 0000000000..3e3a97ff66 --- /dev/null +++ b/test-projects/swerve-heavy/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy/sweep_auto.traj b/test-projects/swerve-heavy/sweep_auto.traj new file mode 100644 index 0000000000..60f3cc0689 --- /dev/null +++ b/test-projects/swerve-heavy/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak/full_field_sweeper.traj b/test-projects/swerve-weak/full_field_sweeper.traj new file mode 100644 index 0000000000..b21c0ce67b --- /dev/null +++ b/test-projects/swerve-weak/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak/project.chor b/test-projects/swerve-weak/project.chor new file mode 100755 index 0000000000..e16c369e91 --- /dev/null +++ b/test-projects/swerve-weak/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2.0", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5800 RPM", + "val":607.3745796940267 + }, + "stall_torque":{ + "exp":"4.20 N * m", + "val":4.20 + }, + "kT":{ + "exp":"0.02 N * m / A", + "val":0.02 + }, + "kV":{ + "exp":"0.01975716537568162 V / rpm", + "val":0.01975716537568162 + }, + "supply_limit":{ + "exp":"50 A", + "val":50.0 + }, + "stator_limit":{ + "exp":"50 A", + "val":50.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-weak/small_curve.traj b/test-projects/swerve-weak/small_curve.traj new file mode 100644 index 0000000000..d1c246e26d --- /dev/null +++ b/test-projects/swerve-weak/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak/straight_line.traj b/test-projects/swerve-weak/straight_line.traj new file mode 100644 index 0000000000..3e3a97ff66 --- /dev/null +++ b/test-projects/swerve-weak/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak/sweep_auto.traj b/test-projects/swerve-weak/sweep_auto.traj new file mode 100644 index 0000000000..60f3cc0689 --- /dev/null +++ b/test-projects/swerve-weak/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.02 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.02 s", + "val":0.02 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} From bae0239d6e4c7ffe1443fff60fef5ce0444a0822 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 04:04:18 -0400 Subject: [PATCH 02/53] increase sccache timeout skull --- .github/workflows/benchmark.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index cef47db948..ee84d930c4 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -85,6 +85,7 @@ jobs: env: RUSTC_WRAPPER: sccache SCCACHE_GHA_ENABLED: "true" + SCCACHE_STARTUP_TIMEOUT: "600" - name: Stash PR binary run: cp target/release/choreo-cli /tmp/cli-pr @@ -112,6 +113,7 @@ jobs: env: RUSTC_WRAPPER: sccache SCCACHE_GHA_ENABLED: "true" + SCCACHE_STARTUP_TIMEOUT: "600" - name: Stash base binary run: cp target/release/choreo-cli /tmp/cli-base From 0c1757490bea9eaa55350a550ef2aa4a04740d43 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 04:29:14 -0400 Subject: [PATCH 03/53] bump to GCC14 --- .github/workflows/benchmark.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index ee84d930c4..4bcc5ae72f 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -62,6 +62,11 @@ jobs: fetch-depth: 0 persist-credentials: false + - name: Make GCC 14 the default toolchain + run: | + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 200 + sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 200 + - name: Install Linux dependencies run: | sudo apt-get update -q From 80a977b4e0add708f220dd8c6fac24203ce45dd8 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 04:46:07 -0400 Subject: [PATCH 04/53] clarify CI name --- .github/workflows/benchmark.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index 4bcc5ae72f..06fe67cea8 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -1,4 +1,4 @@ -name: PR Benchmark +name: Solver Time Benchmark # Comment-triggered: a maintainer (OWNER / MEMBER / COLLABORATOR) types # `/bench` on a PR and this workflow builds the PR's choreo-cli and the base @@ -132,7 +132,7 @@ jobs: run: bash scripts/run-bench.sh /tmp/cli-base /tmp/tp-base /tmp/out-base continue-on-error: true # tolerate schema-incompatible base - - name: Render trajectory SVGs (per-step accel coloring) + - name: Render trajectory SVGs with linear acceleration coloring run: | set -euo pipefail render() { From b62e1ed64e8c17831d6e1cfbee626cb2100c591d Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 05:58:07 -0400 Subject: [PATCH 05/53] remove hyperfine warmup --- scripts/run-bench.sh | 1 - 1 file changed, 1 deletion(-) diff --git a/scripts/run-bench.sh b/scripts/run-bench.sh index 5f18754e96..c6a2a60895 100755 --- a/scripts/run-bench.sh +++ b/scripts/run-bench.sh @@ -29,7 +29,6 @@ for proj_dir in "${project_dirs[@]}"; do fi echo "::group::$name" hyperfine \ - --warmup 1 \ --runs 3 \ --export-json "$OUT/$name.hyperfine.json" \ "$CLI --chor $chor --all-trajectory --generate --report-json $OUT/$name.report.json" From 916033873db2766c8ad2bae61dabfb40895cf2bf Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 08:45:54 -0400 Subject: [PATCH 06/53] parallelize, revamp bench CI --- .github/workflows/benchmark.yml | 252 +++++++++++++++------ scripts/bench-report.mjs | 365 ++++++++++++++++++++++--------- scripts/ci/enumerate-variants.sh | 18 ++ scripts/ci/publish-renders.cjs | 55 +++++ scripts/ci/render-variant.sh | 22 ++ scripts/run-bench.sh | 37 +++- 6 files changed, 569 insertions(+), 180 deletions(-) create mode 100755 scripts/ci/enumerate-variants.sh create mode 100644 scripts/ci/publish-renders.cjs create mode 100755 scripts/ci/render-variant.sh diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index 06fe67cea8..081716d28a 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -1,11 +1,22 @@ name: Solver Time Benchmark # Comment-triggered: a maintainer (OWNER / MEMBER / COLLABORATOR) types -# `/bench` on a PR and this workflow builds the PR's choreo-cli and the base -# branch's choreo-cli, runs both against the PR's test-projects/, computes -# per-trajectory solve-time / length / duration deltas, renders each trajectory -# as an SVG with per-step linear-acceleration coloring, uploads everything as -# an artifact, and posts a sticky PR comment with the comparison table. +# `/bench` on a PR. The work is split across parallel jobs to cut wall-clock: +# +# resolve ─ gate + ack + resolve PR head/base SHAs +# │ +# ├─ build (matrix: pr | base) ── two self-contained choreo-cli binaries +# │ built concurrently, uploaded as artifacts +# │ +# └─ bench (matrix: one job per project variant) ── each downloads both +# binaries, benches ONLY its variant, renders that variant's +# SVGs, uploads a per-variant artifact +# │ +# collate ─ merges every per-variant artifact, pushes the rendered SVGs to a +# per-run `bench-renders/` ref (so they're viewable in the +# browser via GitHub's blob viewer, not just a ZIP), builds the +# comparison report, uploads the consolidated artifact, posts a +# fresh PR comment on: issue_comment: @@ -13,15 +24,11 @@ on: permissions: pull-requests: write - contents: read + contents: write # collate pushes trajectory SVGs to a per-run bench-renders/* ref actions: read -concurrency: - group: bench-${{ github.event.issue.number }} - cancel-in-progress: true - jobs: - bench: + resolve: # Only run when: # - the comment is on a PR (not a plain issue) # - the body starts with "/bench" @@ -33,6 +40,11 @@ jobs: github.event.comment.author_association == 'MEMBER' || github.event.comment.author_association == 'COLLABORATOR') runs-on: ubuntu-24.04 + outputs: + head_sha: ${{ steps.pr.outputs.head_sha }} + base_sha: ${{ steps.pr.outputs.base_sha }} + head_repo: ${{ steps.pr.outputs.head_repo }} + variants: ${{ steps.variants.outputs.variants }} steps: - name: Acknowledge the trigger comment uses: peter-evans/create-or-update-comment@v4 @@ -54,14 +66,47 @@ jobs: core.setOutput('base_sha', pr.data.base.sha); core.setOutput('head_repo', pr.data.head.repo.full_name); - - name: Checkout PR head + - name: Checkout test-projects only uses: actions/checkout@v6 with: repository: ${{ steps.pr.outputs.head_repo }} ref: ${{ steps.pr.outputs.head_sha }} + persist-credentials: false + sparse-checkout: test-projects + sparse-checkout-cone-mode: false + + - name: Enumerate project variants + id: variants + run: bash scripts/ci/enumerate-variants.sh + + # -------------------------------------------------------------------------- + # build choreo-cli for both PR and base concurrently + build: + needs: resolve + runs-on: ubuntu-24.04 + strategy: + fail-fast: false + matrix: + side: [pr, base] + steps: + - name: Checkout PR head + uses: actions/checkout@v6 + with: + repository: ${{ needs.resolve.outputs.head_repo }} + ref: ${{ needs.resolve.outputs.head_sha }} fetch-depth: 0 persist-credentials: false + # For the base binary, overlay the base branch's Rust crates on top of + # the PR checkout (everything else — scripts, test-projects — is unused + # by the build itself). + - name: Overlay base sources + if: matrix.side == 'base' + run: | + git fetch origin ${{ needs.resolve.outputs.base_sha }} + git checkout ${{ needs.resolve.outputs.base_sha }} -- \ + src-core src-cli trajoptlib Cargo.lock Cargo.toml + - name: Make GCC 14 the default toolchain run: | sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 200 @@ -74,7 +119,6 @@ jobs: build-essential \ curl \ file \ - hyperfine \ libayatana-appindicator3-dev \ librsvg2-dev \ libssl-dev \ @@ -85,96 +129,164 @@ jobs: - name: Set up sccache uses: mozilla-actions/sccache-action@v0.0.10 - - name: Build PR choreo-cli + - name: Build choreo-cli (${{ matrix.side }}) + # if base is schema-incompatible continue anyways + continue-on-error: ${{ matrix.side == 'base' }} run: cargo build --release -p choreo-cli env: RUSTC_WRAPPER: sccache SCCACHE_GHA_ENABLED: "true" SCCACHE_STARTUP_TIMEOUT: "600" - - name: Stash PR binary - run: cp target/release/choreo-cli /tmp/cli-pr + - name: Upload binary + if: always() + uses: actions/upload-artifact@v7 + with: + name: cli-${{ matrix.side }} + path: target/release/choreo-cli + if-no-files-found: warn - - name: Snapshot PR test-projects for both runs + # -------------------------------------------------------------------------- + # parallelize bench for each project + # not parallelized further to avoid variance in HW causing noisy comparisons + bench: + needs: [resolve, build] + runs-on: ubuntu-24.04 + strategy: + fail-fast: false + matrix: + variant: ${{ fromJSON(needs.resolve.outputs.variants) }} + env: + VARIANT: ${{ matrix.variant }} + steps: + - name: Checkout PR head + uses: actions/checkout@v6 + with: + repository: ${{ needs.resolve.outputs.head_repo }} + ref: ${{ needs.resolve.outputs.head_sha }} + persist-credentials: false + + - name: Install Linux dependencies run: | - cp -r test-projects /tmp/tp-pr - cp -r test-projects /tmp/tp-base + sudo apt-get update -q + sudo apt-get install -y \ + build-essential \ + libssl-dev - - name: Run PR benchmark - run: bash scripts/run-bench.sh /tmp/cli-pr /tmp/tp-pr /tmp/out-pr + - name: Download PR binary + uses: actions/download-artifact@v7 + with: + name: cli-pr + path: bin-pr - # Now overlay base-branch sources of just the Rust crates so we can - # build the base binary against PR's test-projects/ + scripts/. - - name: Fetch base ref - run: git fetch origin ${{ steps.pr.outputs.base_sha }} + - name: Download base binary + id: dl_base + continue-on-error: true + uses: actions/download-artifact@v7 + with: + name: cli-base + path: bin-base - - name: Check out base sources (Rust crates only) + - name: Make binaries executable run: | - git checkout ${{ steps.pr.outputs.base_sha }} -- \ - src-core src-cli trajoptlib Cargo.lock Cargo.toml + chmod +x bin-pr/choreo-cli + [ -f bin-base/choreo-cli ] && chmod +x bin-base/choreo-cli || true - - name: Build base choreo-cli - run: cargo build --release -p choreo-cli - env: - RUSTC_WRAPPER: sccache - SCCACHE_GHA_ENABLED: "true" - SCCACHE_STARTUP_TIMEOUT: "600" - - - name: Stash base binary - run: cp target/release/choreo-cli /tmp/cli-base - - - name: Restore PR sources for scripts + - name: Stage variant run: | - git checkout ${{ steps.pr.outputs.head_sha }} -- \ - src-core src-cli trajoptlib Cargo.lock Cargo.toml + mkdir -p tp-pr tp-base + cp -r "test-projects/$VARIANT" "tp-pr/$VARIANT" + cp -r "test-projects/$VARIANT" "tp-base/$VARIANT" + + - name: Run PR benchmark + run: bash scripts/run-bench.sh "$PWD/bin-pr/choreo-cli" tp-pr out-pr "$VARIANT" - name: Run base benchmark - run: bash scripts/run-bench.sh /tmp/cli-base /tmp/tp-base /tmp/out-base - continue-on-error: true # tolerate schema-incompatible base + continue-on-error: true # tolerate missing / schema-incompatible base + run: | + if [ -x bin-base/choreo-cli ]; then + bash scripts/run-bench.sh "$PWD/bin-base/choreo-cli" tp-base out-base "$VARIANT" + else + echo "no base binary available — skipping base run for $VARIANT" + fi - name: Render trajectory SVGs with linear acceleration coloring - run: | - set -euo pipefail - render() { - local side=$1 - local root=$2 - for traj in "$root"/*/*.traj; do - variant=$(basename "$(dirname "$traj")") - name=$(basename "$traj" .traj) - out="${traj%.traj}.${side}.svg" - node scripts/render-traj.mjs --traj "$traj" --out "$out" \ - --title "${side^^} ${variant} · ${name}" || true - done - } - render pr /tmp/tp-pr - render base /tmp/tp-base + run: bash scripts/ci/render-variant.sh + + - name: Upload per-variant artifact + if: always() + uses: actions/upload-artifact@v7 + with: + name: bench-${{ matrix.variant }} + path: | + out-pr/** + out-base/** + tp-pr/** + tp-base/** + if-no-files-found: warn + + # -------------------------------------------------------------------------- + collate: + needs: [resolve, bench] + if: ${{ !cancelled() && needs.resolve.result == 'success' }} + runs-on: ubuntu-24.04 + steps: + - name: Checkout PR head + uses: actions/checkout@v6 + with: + repository: ${{ needs.resolve.outputs.head_repo }} + ref: ${{ needs.resolve.outputs.head_sha }} + persist-credentials: false + + - name: Merge per-variant artifacts + continue-on-error: true # if every bench job was skipped, still comment + uses: actions/download-artifact@v7 + with: + pattern: bench-* + path: merged + merge-multiple: true + + - name: Publish renders to a ref + id: renders + continue-on-error: true # a missing renders link must not block the comment + uses: actions/github-script@v7 + env: + HEAD_SHA: ${{ needs.resolve.outputs.head_sha }} + with: + script: | + const publish = require(`${process.env.GITHUB_WORKSPACE}/scripts/ci/publish-renders.cjs`) + await publish({ github, context, core }) - name: Build markdown report env: ARTIFACT_URL: ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }} + HEAD_SHA: ${{ needs.resolve.outputs.head_sha }} + RENDERS_URL: ${{ steps.renders.outputs.url }} run: | node scripts/bench-report.mjs \ - --pr /tmp/tp-pr --base /tmp/tp-base \ - --pr-reports /tmp/out-pr --base-reports /tmp/out-base \ + --pr merged/tp-pr --base merged/tp-base \ + --pr-reports merged/out-pr --base-reports merged/out-base \ --artifact-url "$ARTIFACT_URL" \ + ${RENDERS_URL:+--renders-url "$RENDERS_URL"} \ + --commit "${HEAD_SHA:0:7}" \ --out report.md cat report.md - - name: Upload bench artifact + - name: Upload consolidated bench artifact if: always() uses: actions/upload-artifact@v7 with: name: bench-output-${{ github.run_id }} path: | - /tmp/out-pr/** - /tmp/out-base/** - /tmp/tp-pr/**/*.svg - /tmp/tp-base/**/*.svg + merged/out-pr/** + merged/out-base/** + merged/tp-pr/**/*.svg + merged/tp-base/**/*.svg report.md + if-no-files-found: warn - - name: Post sticky comment on PR - uses: marocchino/sticky-pull-request-comment@v2 + - name: Post benchmark comment on PR + uses: peter-evans/create-or-update-comment@v4 with: - header: bench - number: ${{ github.event.issue.number }} - path: report.md + issue-number: ${{ github.event.issue.number }} + body-path: report.md diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index 91d857c7a4..618655b999 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -1,11 +1,18 @@ #!/usr/bin/env node -// Build a markdown comparison report from two `run-bench.sh` outputs. +// Build a markdown comparison report from two `run-bench.sh` output dirs. // // Usage: // node scripts/bench-report.mjs \ // --pr --base \ // --pr-reports --base-reports \ -// --artifact-url --out +// --artifact-url --commit --out +// +// Each *-reports dir holds one CLI report per run: `.run.report.json` +// (an array of {name, solve_ms, ok, error}). We aggregate the runs per +// trajectory into mean ± stdev. Trajectory geometry (length / duration / +// sample count) is read from the staged `/.traj` ONLY when that +// side actually generated it this run (report says ok) — otherwise the `.traj` +// on disk is stale committed input, not this run's output. import { readFileSync, writeFileSync, existsSync, readdirSync, statSync } from "node:fs"; import { join } from "node:path"; @@ -18,65 +25,86 @@ for (const k of ["pr", "base", "pr-reports", "base-reports", "out"]) { } } const artifactUrl = argv["artifact-url"] ?? null; +const commit = argv.commit ?? null; +// Base blob URL of the per-run bench-renders ref; SVGs live at +// //.{pr,base}.svg (matches the bench job's +// `${traj%.traj}.${side}.svg` render paths). +const rendersUrl = argv["renders-url"] ?? null; -// Thresholds for highlighting regressions. -const SOLVE_PCT_THRESHOLD = 15; // bold/red if |pctSolve| > 15% -const LEN_DELTA_THRESHOLD = 0.01; // bold/red if |dLen| > 1 cm +// Solve-time regression highlight threshold (bold if |Δ%| exceeds this). +const SOLVE_PCT_THRESHOLD = 15; const variants = listVariants(argv.pr); const rows = []; -const summary = { regressed: 0, improved: 0, base_failed: 0, total: 0 }; +const summary = { + total: 0, // trajectories seen across all variants + comparable: 0, // generated OK on BOTH sides (the only ones scored) + prFailed: 0, // did not generate on PR + baseFailed: 0, // generated on PR but not on base (incl. base missing) + flaky: 0, // some-but-not-all runs OK on a side + sumPr: 0, // Σ mean solve ms over comparable + sumBase: 0, + pcts: [], // per-trajectory solve Δ% over comparable + dTimeTotal: 0, // Σ trajectory-duration Δ over comparable + dTimeCount: 0, +}; for (const variant of variants) { - const prReport = loadReport(argv["pr-reports"], variant); - const baseReport = loadReport(argv["base-reports"], variant); - const prDir = join(argv.pr, variant); - const baseDir = join(argv.base, variant); + const prRuns = loadRuns(argv["pr-reports"], variant); + const baseRuns = loadRuns(argv["base-reports"], variant); - if (!prReport) { - rows.push(missingRow(variant, "(no PR report)")); + if (prRuns.length === 0) { + rows.push({ variant, noReport: true }); continue; } - for (const prEntry of prReport) { - const name = prEntry.name; - const baseEntry = baseReport?.find(r => r.name === name) ?? null; - const prTraj = trajMetrics(join(prDir, `${name}.traj`)); - const baseTraj = trajMetrics(join(baseDir, `${name}.traj`)); - - let status = "OK"; - if (!prEntry.ok) status = "PR_FAILED"; - else if (!baseEntry) status = "BASE_MISSING"; - else if (!baseEntry.ok) status = "BASE_FAILED"; - - const solvePr = prEntry.ok ? prEntry.solve_ms : null; - const solveBase = baseEntry?.ok ? baseEntry.solve_ms : null; - const dSolve = (solvePr != null && solveBase != null) ? solvePr - solveBase : null; - const pctSolve = (dSolve != null && solveBase > 0) ? (dSolve / solveBase) * 100 : null; - const dLen = (prTraj && baseTraj) ? prTraj.length_m - baseTraj.length_m : null; - const dTime = (prTraj && baseTraj) ? prTraj.total_s - baseTraj.total_s : null; - - const regressed = - status !== "OK" || - (pctSolve != null && Math.abs(pctSolve) > SOLVE_PCT_THRESHOLD) || - (dLen != null && Math.abs(dLen) > LEN_DELTA_THRESHOLD); + const names = [...new Set(prRuns.flatMap(r => r.map(e => e?.name).filter(Boolean)))]; + for (const name of names) { + const pr = aggregateSide(prRuns, name); + const base = aggregateSide(baseRuns, name); + const prOk = !!(pr && pr.ok); + const baseOk = !!(base && base.ok); + + // Only trust geometry from a side that actually produced this traj. + const prTraj = prOk ? trajMetrics(join(argv.pr, variant, `${name}.traj`)) : null; + const baseTraj = baseOk ? trajMetrics(join(argv.base, variant, `${name}.traj`)) : null; + + let status; + if (!prOk) status = "PR_FAILED"; + else if (!base || base.runs === 0) status = "BASE_MISSING"; + else if (!baseOk) status = "BASE_FAILED"; + else if (pr.flaky || base.flaky) status = "FLAKY"; + else status = "OK"; + + const dSolve = prOk && baseOk ? pr.mean - base.mean : null; + const pct = dSolve != null && base.mean > 0 ? (dSolve / base.mean) * 100 : null; + const dLen = prTraj && baseTraj ? prTraj.length_m - baseTraj.length_m : null; + const dTime = prTraj && baseTraj ? prTraj.total_s - baseTraj.total_s : null; summary.total++; - if (status === "BASE_MISSING" || status === "BASE_FAILED") summary.base_failed++; - else if (regressed && pctSolve != null && pctSolve > 0) summary.regressed++; - else if (pctSolve != null && pctSolve < -SOLVE_PCT_THRESHOLD) summary.improved++; - - rows.push({ variant, name, status, regressed, - solvePr, solveBase, dSolve, pctSolve, - lenPr: prTraj?.length_m, lenBase: baseTraj?.length_m, dLen, - timePr: prTraj?.total_s, timeBase: baseTraj?.total_s, dTime, - samplesPr: prTraj?.sample_count, samplesBase: baseTraj?.sample_count }); + if (!prOk) summary.prFailed++; + else if (!baseOk) summary.baseFailed++; + else { + summary.comparable++; + summary.sumPr += pr.mean; + summary.sumBase += base.mean; + if (pct != null) summary.pcts.push(pct); + if (dTime != null) { summary.dTimeTotal += dTime; summary.dTimeCount++; } + } + if ((pr && pr.flaky) || (base && base.flaky)) summary.flaky++; + + rows.push({ + variant, name, status, pr, base, prOk, baseOk, dSolve, pct, + lenPr: prTraj?.length_m, lenBase: baseTraj?.length_m, dLen, + timePr: prTraj?.total_s, timeBase: baseTraj?.total_s, dTime, + samplesPr: prTraj?.sample_count, samplesBase: baseTraj?.sample_count, + }); } } -writeFileSync(argv.out, renderMarkdown(rows, summary, artifactUrl)); +writeFileSync(argv.out, renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl)); -// ---------- helpers ---------- +// ---------- aggregation ---------- function listVariants(dir) { if (!existsSync(dir)) return []; @@ -86,11 +114,43 @@ function listVariants(dir) { .sort(); } -function loadReport(dir, variant) { - const p = join(dir, `${variant}.report.json`); - if (!existsSync(p)) return null; - try { return JSON.parse(readFileSync(p, "utf8")); } - catch { return null; } +// All per-run report arrays for a variant: `.run.report.json`, +// plus a legacy single `.report.json` if present. +function loadRuns(dir, variant) { + if (!existsSync(dir)) return []; + const files = readdirSync(dir) + .filter(f => + f === `${variant}.report.json` || + (f.startsWith(`${variant}.run`) && f.endsWith(".report.json"))) + .sort(); + const runs = []; + for (const f of files) { + try { + const j = JSON.parse(readFileSync(join(dir, f), "utf8")); + if (Array.isArray(j)) runs.push(j); + } catch { /* skip unparseable run */ } + } + return runs; +} + +function aggregateSide(runs, name) { + const entries = runs.map(r => r.find(e => e && e.name === name)).filter(Boolean); + if (entries.length === 0) return null; + const okEntries = entries.filter(e => e.ok); + const samples = okEntries + .map(e => e.solve_ms) + .filter(v => typeof v === "number" && Number.isFinite(v)); + const ok = samples.length > 0; + const mean = ok ? samples.reduce((a, b) => a + b, 0) / samples.length : null; + const sd = ok && samples.length > 1 + ? Math.sqrt(samples.reduce((a, b) => a + (b - mean) ** 2, 0) / (samples.length - 1)) + : 0; + const error = entries.filter(e => !e.ok).map(e => e.error).filter(Boolean).pop() ?? null; + return { + ok, + flaky: ok && okEntries.length < entries.length, + mean, sd, n: samples.length, runs: entries.length, error, + }; } function trajMetrics(trajPath) { @@ -107,74 +167,179 @@ function trajMetrics(trajPath) { return { length_m: len, total_s: samples[samples.length - 1].t, sample_count: samples.length }; } -function missingRow(variant, note) { - return { variant, name: note, status: "NO_PR_REPORT", regressed: true }; -} +// ---------- rendering ---------- + +function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { + const L = []; + L.push(commit + ? `### Choreo trajectory benchmark — commit \`${commit}\`` + : `### Choreo trajectory benchmark`); + L.push(""); + + if (summary.comparable > 0) { + const overall = ((summary.sumPr - summary.sumBase) / summary.sumBase) * 100; + const med = median(summary.pcts); + L.push(`**Solve time ${signedPct(overall)} overall** — PR ${fmtTotal(summary.sumPr)} vs base ${fmtTotal(summary.sumBase)} over ${summary.comparable} comparable trajector${summary.comparable === 1 ? "y" : "ies"} (median per-trajectory ${signedPct(med)}).`); + L.push(""); + L.push(`Trajectory duration: ${signedFixed(summary.dTimeTotal, 3)} s total across ${summary.dTimeCount} compared. Path-length differences are diagnostic only — not a pass/fail signal.`); + } else { + L.push(`**No trajectories generated successfully on both PR and base** — nothing to score. See per-variant statuses below.`); + } + L.push(""); + L.push(`${summary.comparable} comparable · ${summary.prFailed} failed on PR · ${summary.baseFailed} failed on base · ${summary.flaky} flaky · ${summary.total} total`); + L.push(""); -function renderMarkdown(rows, summary, artifactUrl) { - const lines = []; - lines.push(`### Choreo trajectory benchmark`); - lines.push(""); - lines.push(`Compared ${summary.total} trajectories across ${new Set(rows.map(r => r.variant)).size} project variant(s).`); - const bits = []; - if (summary.regressed) bits.push(`**${summary.regressed} regressed** (solve time >${SOLVE_PCT_THRESHOLD}% slower or path length shifted >${LEN_DELTA_THRESHOLD}m)`); - if (summary.improved) bits.push(`${summary.improved} sped up`); - if (summary.base_failed) bits.push(`${summary.base_failed} unable to compare to base`); - lines.push(bits.length ? bits.join(" · ") : "No notable differences."); - lines.push(""); - lines.push("| Variant | Trajectory | Solve (PR) | Solve (base) | Δsolve | Length Δ | Time Δ | Samples (PR/base) | Status |"); - lines.push("|---|---|---:|---:|---:|---:|---:|:---:|:---:|"); + // Group rows by variant (listVariants order is preserved by push order). + const byVariant = new Map(); for (const r of rows) { - if (r.status === "NO_PR_REPORT") { - lines.push(`| \`${r.variant}\` | — | — | — | — | — | — | — | **${r.status}** |`); + if (!byVariant.has(r.variant)) byVariant.set(r.variant, []); + byVariant.get(r.variant).push(r); + } + + // Auto-expand the single worst-regressed variant so reviewers see detail + // without clicking. + let worstVariant = null; + let worstPct = -Infinity; + for (const [v, rs] of byVariant) { + const cmp = rs.filter(r => r.status === "OK" || r.status === "FLAKY"); + if (cmp.length === 0) continue; + const sp = cmp.reduce((a, r) => a + r.pr.mean, 0); + const sb = cmp.reduce((a, r) => a + r.base.mean, 0); + if (sb > 0) { + const p = ((sp - sb) / sb) * 100; + if (p > worstPct) { worstPct = p; worstVariant = v; } + } + } + + for (const [variant, rs] of byVariant) { + if (rs.length === 1 && rs[0].noReport) { + L.push(`
${variant} — ⚠ no PR report`); + L.push(""); + L.push(`The PR CLI produced no report for this variant (it crashed or was killed before writing one).`); + L.push(""); + L.push(`
`); + L.push(""); continue; } - lines.push("| " + [ - `\`${r.variant}\``, - `\`${r.name}\``, - fmtMs(r.solvePr), - fmtMs(r.solveBase), - fmtSolveDelta(r.dSolve, r.pctSolve, r.regressed), - fmtLenDelta(r.dLen, r.regressed), - fmtTimeDelta(r.dTime), - r.samplesPr != null && r.samplesBase != null ? `${r.samplesPr}/${r.samplesBase}` : (r.samplesPr ?? "—"), - r.status === "OK" ? "OK" : `**${r.status}**`, - ].join(" | ") + " |"); + + const cmp = rs.filter(r => r.status === "OK" || r.status === "FLAKY"); + let verdict; + if (cmp.length > 0) { + const sp = cmp.reduce((a, r) => a + r.pr.mean, 0); + const sb = cmp.reduce((a, r) => a + r.base.mean, 0); + verdict = `solve ${signedPct(sb > 0 ? ((sp - sb) / sb) * 100 : 0)} (${cmp.length}/${rs.length} comparable)`; + } else { + verdict = `0/${rs.length} comparable`; + } + const open = variant === worstVariant ? " open" : ""; + L.push(`${variant} — ${verdict}`); + L.push(""); + L.push(`| Trajectory | Solve (PR) | Solve (base) | Δsolve | Δlength | Δtime | Samples PR/base | Status |`); + L.push(`|---|---:|---:|---:|---:|---:|:---:|:---:|`); + for (const r of rs) { + L.push("| " + [ + `\`${r.name}\``, + fmtSolve(r.pr), + fmtSolve(r.base), + fmtDeltaSolve(r.dSolve, r.pct), + fmtMeters(r.dLen), + fmtSeconds(r.dTime), + `${r.samplesPr ?? "—"}/${r.samplesBase ?? "—"}`, + fmtStatus(r.status), + ].join(" | ") + " |"); + } + L.push(""); + + if (rendersUrl) { + const items = []; + for (const r of rs) { + const segs = [`${encodeURIComponent(r.variant)}/${encodeURIComponent(r.name)}`]; + const links = []; + if (r.prOk) links.push(`[PR](${rendersUrl}/${segs}.pr.svg)`); + if (r.baseOk) links.push(`[base](${rendersUrl}/${segs}.base.svg)`); + if (links.length) items.push(`\`${r.name}\` — ${links.join(" · ")}`); + } + if (items.length) { + L.push(`Renders (open in browser): ${items.join(" — ")}`); + L.push(""); + } + } + + L.push(``); + L.push(""); } - lines.push(""); + if (artifactUrl) { - lines.push(`Trajectory renderings (per-step linear-acceleration coloring, matching the UI gradient) are uploaded as a workflow artifact — open the run page to download:`); - lines.push(""); - lines.push(`→ **[Download bench-output artifact](${artifactUrl})**`); + L.push(`Trajectory renderings (per-step linear-acceleration coloring, matching the UI gradient) are uploaded as a workflow artifact:`); + L.push(""); + L.push(`→ **[Download bench-output artifact](${artifactUrl})**`); + L.push(""); } - lines.push(""); - lines.push(`Solve times are wall-clock around \`generate()\` only (excluding CLI startup), measured with hyperfine \`--warmup 1 --runs 3\` and reported by the CLI's \`--report-json\`. \`Length Δ\` is the change in integrated path length (sum of segment lengths). \`Time Δ\` is the change in total trajectory duration. \`Samples\` shows the count from each side; very different counts mean point-wise comparisons would be misleading — use the artifact's SVGs for visual diff.`); - return lines.join("\n") + "\n"; + L.push(`Solve times are wall-clock around \`generate()\` only (excluding CLI startup), reported by the CLI's \`--report-json\` and aggregated as mean ± sample stdev over independent CLI runs. \`Δlength\`/\`Δtime\` are shown only when a trajectory generated successfully on **both** sides; path length is diagnostic, not a pass/fail signal. \`Samples\` is the per-side sample count (\`—\` = that side did not generate it).`); + return L.join("\n") + "\n"; +} + +// ---------- formatting ---------- + +function unitFor(ms) { return Math.abs(ms) >= 10000 ? "s" : "ms"; } +function inUnit(ms, u) { return u === "s" ? ms / 1000 : ms; } +function decimals(u) { return u === "s" ? 2 : 1; } + +function fmtTotal(ms) { + const u = unitFor(ms); + return `${inUnit(ms, u).toFixed(decimals(u))} ${u}`; } -function fmtMs(v) { - if (v == null) return "—"; - return `${v.toFixed(1)} ms`; +function fmtSolve(s) { + if (!s || !s.ok || s.mean == null) return "—"; + const u = unitFor(s.mean); + const m = inUnit(s.mean, u).toFixed(decimals(u)); + if (s.n > 1 && s.sd > 0) { + return `${m} ± ${inUnit(s.sd, u).toFixed(decimals(u))} ${u}`; + } + return `${m} ${u}`; } -function fmtSolveDelta(d, pct, regressed) { +function fmtDeltaSolve(d, pct) { if (d == null) return "—"; - const sign = d > 0 ? "+" : ""; - const txt = `${sign}${d.toFixed(1)} ms${pct != null ? ` (${sign}${pct.toFixed(1)}%)` : ""}`; - return regressed && pct != null && Math.abs(pct) > SOLVE_PCT_THRESHOLD ? `**${txt}**` : txt; + const u = unitFor(d); + const sign = d >= 0 ? "+" : "-"; + const mag = Math.abs(inUnit(d, u)).toFixed(decimals(u)); + const p = pct != null ? ` (${signedPct(pct)})` : ""; + const txt = `${sign}${mag} ${u}${p}`; + return pct != null && Math.abs(pct) > SOLVE_PCT_THRESHOLD ? `**${txt}**` : txt; } -function fmtLenDelta(d, regressed) { +function fmtMeters(d) { if (d == null) return "—"; - const sign = d >= 0 ? "+" : ""; - const txt = `${sign}${d.toFixed(4)} m`; - return regressed && Math.abs(d) > LEN_DELTA_THRESHOLD ? `**${txt}**` : txt; + return `${d >= 0 ? "+" : "-"}${Math.abs(d).toFixed(4)} m`; } -function fmtTimeDelta(d) { +function fmtSeconds(d) { if (d == null) return "—"; - const sign = d >= 0 ? "+" : ""; - return `${sign}${d.toFixed(3)} s`; + return `${signedFixed(d, 3)} s`; +} + +function fmtStatus(s) { + if (s === "OK") return "OK"; + if (s === "FLAKY") return "⚠ FLAKY"; + return `**${s}**`; +} + +function signedPct(p) { + if (p == null || !Number.isFinite(p)) return "n/a"; + return `${p >= 0 ? "+" : "-"}${Math.abs(p).toFixed(1)}%`; +} + +function signedFixed(v, dp) { + return `${v >= 0 ? "+" : "-"}${Math.abs(v).toFixed(dp)}`; +} + +function median(xs) { + if (!xs.length) return null; + const s = [...xs].sort((a, b) => a - b); + const m = s.length >> 1; + return s.length % 2 ? s[m] : (s[m - 1] + s[m]) / 2; } function parseArgs(args) { diff --git a/scripts/ci/enumerate-variants.sh b/scripts/ci/enumerate-variants.sh new file mode 100755 index 0000000000..67ceaca800 --- /dev/null +++ b/scripts/ci/enumerate-variants.sh @@ -0,0 +1,18 @@ +#!/usr/bin/env bash +# CI glue (resolve job): emit a JSON array of test-projects/* variants that +# contain a project.chor as a `variants=[...]` line on $GITHUB_OUTPUT — this +# feeds the dynamic bench matrix. Fails if none are found. +set -euo pipefail + +list=$(for d in test-projects/*/; do + v=$(basename "$d") + [ -f "$d/project.chor" ] && printf '%s\n' "$v" +done | jq -R . | jq -cs .) + +if [ "$list" = "[]" ]; then + echo "no variants with project.chor found under test-projects/" >&2 + exit 1 +fi + +echo "variants=$list" >> "$GITHUB_OUTPUT" +echo "matrix: $list" diff --git a/scripts/ci/publish-renders.cjs b/scripts/ci/publish-renders.cjs new file mode 100644 index 0000000000..a8d2ff50f1 --- /dev/null +++ b/scripts/ci/publish-renders.cjs @@ -0,0 +1,55 @@ +// push renders to orphan ref for viewing + +const fs = require("fs"); +const path = require("path"); + +module.exports = async ({ github, context, core }) => { + const { owner, repo } = context.repo; + const runId = process.env.GITHUB_RUN_ID; + const refName = `bench-renders/${runId}`; + + const tree = []; + for (const root of ["merged/tp-pr", "merged/tp-base"]) { + if (!fs.existsSync(root)) continue; + for (const rel of fs.readdirSync(root, { recursive: true })) { + if (!rel.endsWith(".svg")) continue; + const abs = path.join(root, rel); + if (!fs.statSync(abs).isFile()) continue; + const blob = await github.rest.git.createBlob({ + owner, + repo, + content: fs.readFileSync(abs).toString("base64"), + encoding: "base64", + }); + tree.push({ + path: rel.split(path.sep).join("/"), + mode: "100644", + type: "blob", + sha: blob.data.sha, + }); + } + } + if (tree.length === 0) { + core.info("no SVGs to publish"); + return; + } + + const t = await github.rest.git.createTree({ owner, repo, tree }); + const c = await github.rest.git.createCommit({ + owner, + repo, + tree: t.data.sha, + parents: [], + message: `bench renders — run ${runId} (${(process.env.HEAD_SHA || "").slice(0, 7)})`, + }); + await github.rest.git.createRef({ + owner, + repo, + ref: `refs/heads/${refName}`, + sha: c.data.sha, + }); + core.setOutput( + "url", + `${process.env.GITHUB_SERVER_URL}/${owner}/${repo}/blob/${refName}`, + ); +}; diff --git a/scripts/ci/render-variant.sh b/scripts/ci/render-variant.sh new file mode 100755 index 0000000000..7d73c4a755 --- /dev/null +++ b/scripts/ci/render-variant.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +# CI glue (bench job): render every /*.traj under the PR and base +# staging dirs to a per-step linear-acceleration-colored SVG (one per side). +# VARIANT comes from the job env (the bench matrix entry). A render failure for +# one trajectory must not fail the job, so each is best-effort. +set -euo pipefail + +: "${VARIANT:?VARIANT must be set}" + +render() { + local side=$1 root=$2 + [ -d "$root/$VARIANT" ] || return 0 + for traj in "$root/$VARIANT"/*.traj; do + name=$(basename "$traj" .traj) + node scripts/render-traj.mjs --traj "$traj" \ + --out "${traj%.traj}.${side}.svg" \ + --title "${side^^} ${VARIANT} · ${name}" || true + done +} + +render pr tp-pr +render base tp-base diff --git a/scripts/run-bench.sh b/scripts/run-bench.sh index c6a2a60895..0709c5ae5c 100755 --- a/scripts/run-bench.sh +++ b/scripts/run-bench.sh @@ -1,20 +1,31 @@ #!/usr/bin/env bash -# Run choreo-cli generation across every project variant in $2, using hyperfine -# for repeated-run statistics. The CLI's own --report-json carries the -# authoritative per-trajectory solve_ms (it brackets only generate(), not -# process startup). +# Run choreo-cli generation for one (or every) project variant. # -# Usage: run-bench.sh +# Usage: run-bench.sh [variant] [runs] +# +# If [variant] is given, only that one project subdirectory is benched (used by +# the CI per-variant matrix so each job runs a single variant concurrently). +# Omit it to bench every variant (local / single-job use). [runs] defaults to 3. set -euo pipefail CLI=${1:?missing CLI binary path} PROJECTS=${2:?missing test-projects dir} OUT=${3:?missing output dir} +ONLY_VARIANT=${4:-} +RUNS=${5:-3} mkdir -p "$OUT" shopt -s nullglob -project_dirs=("$PROJECTS"/*/) +if [ -n "$ONLY_VARIANT" ]; then + project_dirs=("$PROJECTS/$ONLY_VARIANT/") + if [ ! -d "${project_dirs[0]}" ]; then + echo "variant '$ONLY_VARIANT' not found under $PROJECTS" >&2 + exit 1 + fi +else + project_dirs=("$PROJECTS"/*/) +fi if [ "${#project_dirs[@]}" -eq 0 ]; then echo "no project subdirectories found under $PROJECTS" >&2 exit 1 @@ -28,9 +39,15 @@ for proj_dir in "${project_dirs[@]}"; do continue fi echo "::group::$name" - hyperfine \ - --runs 3 \ - --export-json "$OUT/$name.hyperfine.json" \ - "$CLI --chor $chor --all-trajectory --generate --report-json $OUT/$name.report.json" + for run in $(seq 1 "$RUNS"); do + echo "--- $name run $run/$RUNS ---" + # A per-trajectory solve failure is reported via ok:false in the JSON and + # the CLI still exits 0; a nonzero exit means a hard failure (bad project, + # crash) — record it and keep going so other runs/variants still produce + # data instead of aborting the whole matrix job. + "$CLI" --chor "$chor" --all-trajectory --generate \ + --report-json "$OUT/$name.run$run.report.json" \ + || echo "choreo-cli exited nonzero on $name run $run/$RUNS" >&2 + done echo "::endgroup::" done From 912d45148c5d477120f92c4d4b54fce6c096fe7b Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 08:53:13 -0400 Subject: [PATCH 07/53] oops, fix CI --- .github/workflows/benchmark.yml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index 081716d28a..f804ae0bcd 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -66,13 +66,17 @@ jobs: core.setOutput('base_sha', pr.data.base.sha); core.setOutput('head_repo', pr.data.head.repo.full_name); - - name: Checkout test-projects only + - name: Checkout test-projects + enumerate script uses: actions/checkout@v6 with: repository: ${{ steps.pr.outputs.head_repo }} ref: ${{ steps.pr.outputs.head_sha }} persist-credentials: false - sparse-checkout: test-projects + # resolve only needs the variant list, so stay sparse — but the + # enumerator now lives in a file, so that path has to be included too. + sparse-checkout: | + test-projects + scripts/ci/enumerate-variants.sh sparse-checkout-cone-mode: false - name: Enumerate project variants From 9b5198f2402954b23131cd80ab619b6dbd976e16 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 10:03:08 -0400 Subject: [PATCH 08/53] fix SVG rendering --- scripts/bench-report.mjs | 22 ++++++++++++++-------- scripts/render-traj.mjs | 30 ++++++++++++++++++++++-------- 2 files changed, 36 insertions(+), 16 deletions(-) diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index 618655b999..cb61d61d0d 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -45,7 +45,7 @@ const summary = { sumPr: 0, // Σ mean solve ms over comparable sumBase: 0, pcts: [], // per-trajectory solve Δ% over comparable - dTimeTotal: 0, // Σ trajectory-duration Δ over comparable + durPcts: [], // per-trajectory duration Δ% over comparable dTimeCount: 0, }; @@ -89,7 +89,10 @@ for (const variant of variants) { summary.sumPr += pr.mean; summary.sumBase += base.mean; if (pct != null) summary.pcts.push(pct); - if (dTime != null) { summary.dTimeTotal += dTime; summary.dTimeCount++; } + if (dTime != null) { + summary.dTimeCount++; + if (baseTraj.total_s > 0) summary.durPcts.push((dTime / baseTraj.total_s) * 100); + } } if ((pr && pr.flaky) || (base && base.flaky)) summary.flaky++; @@ -181,7 +184,7 @@ function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { const med = median(summary.pcts); L.push(`**Solve time ${signedPct(overall)} overall** — PR ${fmtTotal(summary.sumPr)} vs base ${fmtTotal(summary.sumBase)} over ${summary.comparable} comparable trajector${summary.comparable === 1 ? "y" : "ies"} (median per-trajectory ${signedPct(med)}).`); L.push(""); - L.push(`Trajectory duration: ${signedFixed(summary.dTimeTotal, 3)} s total across ${summary.dTimeCount} compared. Path-length differences are diagnostic only — not a pass/fail signal.`); + L.push(`Trajectory duration: median ${signedPct(median(summary.durPcts))} per trajectory across ${summary.dTimeCount} compared.`); } else { L.push(`**No trajectories generated successfully on both PR and base** — nothing to score. See per-variant statuses below.`); } @@ -275,7 +278,7 @@ function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { L.push(`→ **[Download bench-output artifact](${artifactUrl})**`); L.push(""); } - L.push(`Solve times are wall-clock around \`generate()\` only (excluding CLI startup), reported by the CLI's \`--report-json\` and aggregated as mean ± sample stdev over independent CLI runs. \`Δlength\`/\`Δtime\` are shown only when a trajectory generated successfully on **both** sides; path length is diagnostic, not a pass/fail signal. \`Samples\` is the per-side sample count (\`—\` = that side did not generate it).`); + L.push(`Solve times are wall-clock over ${rows.find(r => r.pr?.runs)?.pr.runs ?? "multiple"} runs per trajectory.`); return L.join("\n") + "\n"; } @@ -294,10 +297,13 @@ function fmtSolve(s) { if (!s || !s.ok || s.mean == null) return "—"; const u = unitFor(s.mean); const m = inUnit(s.mean, u).toFixed(decimals(u)); - if (s.n > 1 && s.sd > 0) { - return `${m} ± ${inUnit(s.sd, u).toFixed(decimals(u))} ${u}`; - } - return `${m} ${u}`; + const body = s.n > 1 && s.sd > 0 + ? `${m} ± ${inUnit(s.sd, u).toFixed(decimals(u))} ${u}` + : `${m} ${u}`; + // Make a flaky side identifiable in the table itself (the summary only gives + // a count). Flaky = generated on some but not all runs; the mean is over the + // successful ones only. + return s.flaky ? `${body} ⚠ ${s.n}/${s.runs} ok` : body; } function fmtDeltaSolve(d, pct) { diff --git a/scripts/render-traj.mjs b/scripts/render-traj.mjs index 9773393892..9bdc8e143d 100644 --- a/scripts/render-traj.mjs +++ b/scripts/render-traj.mjs @@ -20,19 +20,33 @@ if (!argv.traj || !argv.out) { // Field dimensions match src/components/field/svg/fields/FieldDimensions.tsx const FIELD_W = 16.541; const FIELD_H = 8.0692; -// Match FieldImage2026.svg's viewBox so embedded background lines up. +// Match FieldImage2026.svg's viewBox so the inlined background lines up. const VB_X = -0.5, VB_Y = -0.5, VB_W = 17.541, VB_H = 9.0692; +// An SVG with only a viewBox renders at the CSS default 300x150 in GitHub's +// blob viewer (hence "tiny"). Give it an explicit intrinsic pixel size. +const PX_PER_M = 150; +const PX_W = Math.round(VB_W * PX_PER_M); +const PX_H = Math.round(VB_H * PX_PER_M); const here = dirname(fileURLToPath(import.meta.url)); const defaultField = resolve(here, "..", "src", "components", "field", "svg", "fields", "FieldImage2026.svg"); const fieldPath = argv["field-svg"] ?? defaultField; -const fieldB64 = readFileSync(fieldPath).toString("base64"); +// Inline the field's vector markup rather than embedding it as a +// `data:image/svg+xml` . GitHub serves raw blobs with CSP +// `default-src 'none'`, which blocks data: URIs — that's why the field +// vanished when the SVG was opened directly. FieldImage2026.svg is pure +// vector (no rasters), and its viewBox already matches ours, so its inner +// content can be dropped straight in. +const fieldInner = readFileSync(fieldPath, "utf8") + .replace(/^[\s\S]*?]*>/i, "") + .replace(/<\/svg>\s*$/i, "") + .trim(); const traj = JSON.parse(readFileSync(argv.traj, "utf8")).trajectory; const samples = traj.samples ?? []; if (samples.length < 2) { - writeFileSync(argv.out, emptySvg(argv.title, fieldB64)); + writeFileSync(argv.out, emptySvg(argv.title, fieldInner)); process.exit(0); } @@ -57,8 +71,8 @@ for (let i = 0; i < samples.length - 1; i++) { // The trajectory uses the same flip transform as FieldImage2026.svg's // internal so we can write raw FRC field coords (origin at // bottom-left) directly into elements. -const out = ` - +const out = ` + ${fieldInner} ${segs.join("\n ")} @@ -73,9 +87,9 @@ function titleElement(title) { return `${escapeXml(title)}`; } -function emptySvg(title, fieldB64) { - return ` - +function emptySvg(title, fieldInner) { + return ` + ${fieldInner} no samples${title ? ` — ${escapeXml(title)}` : ""} `; From 715da3478b505aaec375ee942b6f118b8cb9ba5e Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 10:07:05 -0400 Subject: [PATCH 09/53] put CI on arm for speed? --- .github/workflows/benchmark.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index f804ae0bcd..b6587232cb 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -39,7 +39,7 @@ jobs: (github.event.comment.author_association == 'OWNER' || github.event.comment.author_association == 'MEMBER' || github.event.comment.author_association == 'COLLABORATOR') - runs-on: ubuntu-24.04 + runs-on: ubuntu-24.04-arm outputs: head_sha: ${{ steps.pr.outputs.head_sha }} base_sha: ${{ steps.pr.outputs.base_sha }} @@ -87,7 +87,7 @@ jobs: # build choreo-cli for both PR and base concurrently build: needs: resolve - runs-on: ubuntu-24.04 + runs-on: ubuntu-24.04-arm strategy: fail-fast: false matrix: @@ -155,7 +155,7 @@ jobs: # not parallelized further to avoid variance in HW causing noisy comparisons bench: needs: [resolve, build] - runs-on: ubuntu-24.04 + runs-on: ubuntu-24.04-arm strategy: fail-fast: false matrix: @@ -233,7 +233,7 @@ jobs: collate: needs: [resolve, bench] if: ${{ !cancelled() && needs.resolve.result == 'success' }} - runs-on: ubuntu-24.04 + runs-on: ubuntu-24.04-arm steps: - name: Checkout PR head uses: actions/checkout@v6 From 0b9579a9f3db41b6ff630d349d4f795ec7e64ce1 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 10:17:03 -0400 Subject: [PATCH 10/53] sort names --- scripts/bench-report.mjs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index cb61d61d0d..610e80bba0 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -58,7 +58,8 @@ for (const variant of variants) { continue; } - const names = [...new Set(prRuns.flatMap(r => r.map(e => e?.name).filter(Boolean)))]; + // sort names for consistent order + const names = [...new Set(prRuns.flatMap(r => r.map(e => e?.name).filter(Boolean)))].sort(); for (const name of names) { const pr = aggregateSide(prRuns, name); const base = aggregateSide(baseRuns, name); From 84b6b483f1a57ac403fbcd7f716839b0d1ddf2e4 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 12:11:07 -0400 Subject: [PATCH 11/53] clean up report --- scripts/bench-report.mjs | 15 +++++++-------- scripts/render-traj.mjs | 2 +- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index 610e80bba0..f62f543807 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -39,8 +39,8 @@ const rows = []; const summary = { total: 0, // trajectories seen across all variants comparable: 0, // generated OK on BOTH sides (the only ones scored) - prFailed: 0, // did not generate on PR - baseFailed: 0, // generated on PR but not on base (incl. base missing) + prFailed: 0, // did not generate on PR (independent of base outcome) + baseFailed: 0, // did not generate on base (independent of PR outcome) flaky: 0, // some-but-not-all runs OK on a side sumPr: 0, // Σ mean solve ms over comparable sumBase: 0, @@ -84,8 +84,8 @@ for (const variant of variants) { summary.total++; if (!prOk) summary.prFailed++; - else if (!baseOk) summary.baseFailed++; - else { + if (!baseOk) summary.baseFailed++; + if (prOk && baseOk) { summary.comparable++; summary.sumPr += pr.mean; summary.sumBase += base.mean; @@ -274,12 +274,11 @@ function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { } if (artifactUrl) { - L.push(`Trajectory renderings (per-step linear-acceleration coloring, matching the UI gradient) are uploaded as a workflow artifact:`); - L.push(""); - L.push(`→ **[Download bench-output artifact](${artifactUrl})**`); + L.push(`Trajectory renders can also be found as a workflow artifact:`); + L.push(`→ [Download bench-output artifact](${artifactUrl})`); L.push(""); } - L.push(`Solve times are wall-clock over ${rows.find(r => r.pr?.runs)?.pr.runs ?? "multiple"} runs per trajectory.`); + L.push(`Solve times are wall-clock averaged over ${rows.find(r => r.pr?.runs)?.pr.runs ?? "multiple"} runs per trajectory.`); return L.join("\n") + "\n"; } diff --git a/scripts/render-traj.mjs b/scripts/render-traj.mjs index 9bdc8e143d..1d5f5e815e 100644 --- a/scripts/render-traj.mjs +++ b/scripts/render-traj.mjs @@ -24,7 +24,7 @@ const FIELD_H = 8.0692; const VB_X = -0.5, VB_Y = -0.5, VB_W = 17.541, VB_H = 9.0692; // An SVG with only a viewBox renders at the CSS default 300x150 in GitHub's // blob viewer (hence "tiny"). Give it an explicit intrinsic pixel size. -const PX_PER_M = 150; +const PX_PER_M = 400; const PX_W = Math.round(VB_W * PX_PER_M); const PX_H = Math.round(VB_H * PX_PER_M); From 586821e9f834ae91b65cf3120dfcd38e1b75f69b Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 23:43:51 -0400 Subject: [PATCH 12/53] svg too big go back --- scripts/render-traj.mjs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/render-traj.mjs b/scripts/render-traj.mjs index 1d5f5e815e..9bdc8e143d 100644 --- a/scripts/render-traj.mjs +++ b/scripts/render-traj.mjs @@ -24,7 +24,7 @@ const FIELD_H = 8.0692; const VB_X = -0.5, VB_Y = -0.5, VB_W = 17.541, VB_H = 9.0692; // An SVG with only a viewBox renders at the CSS default 300x150 in GitHub's // blob viewer (hence "tiny"). Give it an explicit intrinsic pixel size. -const PX_PER_M = 400; +const PX_PER_M = 150; const PX_W = Math.round(VB_W * PX_PER_M); const PX_H = Math.round(VB_H * PX_PER_M); From 2dddf8b324190f99b24d947b069158179353698c Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Fri, 15 May 2026 23:50:54 -0400 Subject: [PATCH 13/53] add wpt rendering --- scripts/render-traj.mjs | 47 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/scripts/render-traj.mjs b/scripts/render-traj.mjs index 9bdc8e143d..55ce49880a 100644 --- a/scripts/render-traj.mjs +++ b/scripts/render-traj.mjs @@ -68,13 +68,30 @@ for (let i = 0; i < samples.length - 1; i++) { ); } +// Each entry in traj.waypoints is a timestamp; it sits exactly on a sample +// boundary (see src-core .../transformers/mod.rs postprocess), so the nearest +// sample by `t` gives the waypoint's pose (x, y, heading) on the drawn path. +const cfg = traj.config ?? {}; +const wpTimes = traj.waypoints ?? []; +const markers = wpTimes.map(t => { + let best = samples[0]; + let bestD = Infinity; + for (const s of samples) { + const d = Math.abs(s.t - t); + if (d < bestD) { bestD = d; best = s; } + } + return waypointMarker(best, cfg); +}); + // The trajectory uses the same flip transform as FieldImage2026.svg's // internal so we can write raw FRC field coords (origin at -// bottom-left) directly into elements. +// bottom-left) directly into elements. Waypoint markers are drawn after +// the path segments so the bumper box / heading arrow stay visible on top. const out = ` ${fieldInner} ${segs.join("\n ")} + ${markers.join("\n ")} ${titleElement(argv.title)} @@ -95,6 +112,34 @@ function emptySvg(title, fieldInner) { `; } +// A waypoint, drawn as the robot's bumper footprint (from trajectory.config — +// front/back/side distances from center) rotated to the waypoint heading, plus +// an arrow showing the facing direction. Coords are computed in raw FRC space +// (x forward, y left, CCW heading) so they drop straight into the flipped . +function waypointMarker(s, cfg) { + const h = s.heading ?? 0; + const c = Math.cos(h); + const sn = Math.sin(h); + const b = cfg.bumper ?? {}; + const f = b.front ?? 0.4; + const bk = b.back ?? 0.4; + const sd = b.side ?? 0.4; + // body-frame point (bx forward, by left) -> rotated + translated world point + const rot = (bx, by) => [s.x + bx * c - by * sn, s.y + bx * sn + by * c]; + const pt = ([px, py]) => `${fmt(px)},${fmt(py)}`; + const box = [rot(f, sd), rot(f, -sd), rot(-bk, -sd), rot(-bk, sd)].map(pt).join(" "); + // Arrow shrunk to ~25% of its original size (75% reduction). + const A = 0.25; + const tip = rot(f + 0.35 * A, 0); + const headL = rot(f + 0.05 * A, 0.14 * A); + const headR = rot(f + 0.05 * A, -0.14 * A); + return ( + `` + + `` + + `` + ); +} + function fmt(n) { return Number.isFinite(n) ? n.toFixed(4) : "0"; } From 0dae51dcfe3a8cf2874972bea418ad158ae1b5e7 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 00:59:06 -0400 Subject: [PATCH 14/53] move CI renders to hidden ref --- .github/workflows/benchmark.yml | 11 ++++++----- scripts/bench-report.mjs | 4 ++-- scripts/ci/publish-renders.cjs | 12 +++++++++--- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index b6587232cb..bff7f12373 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -13,10 +13,11 @@ name: Solver Time Benchmark # SVGs, uploads a per-variant artifact # │ # collate ─ merges every per-variant artifact, pushes the rendered SVGs to a -# per-run `bench-renders/` ref (so they're viewable in the -# browser via GitHub's blob viewer, not just a ZIP), builds the -# comparison report, uploads the consolidated artifact, posts a -# fresh PR comment +# per-run CUSTOM ref `refs/bench-renders/` (a hidden ref, +# not a branch/tag, so it never shows in the Branches/Tags UI) and +# links them by commit SHA so they're viewable in the browser via +# GitHub's blob viewer (not just a ZIP), builds the comparison +# report, uploads the consolidated artifact, posts a fresh PR comment on: issue_comment: @@ -24,7 +25,7 @@ on: permissions: pull-requests: write - contents: write # collate pushes trajectory SVGs to a per-run bench-renders/* ref + contents: write # collate pushes trajectory SVGs to a per-run refs/bench-renders/* custom ref actions: read jobs: diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index f62f543807..1481539e35 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -26,8 +26,8 @@ for (const k of ["pr", "base", "pr-reports", "base-reports", "out"]) { } const artifactUrl = argv["artifact-url"] ?? null; const commit = argv.commit ?? null; -// Base blob URL of the per-run bench-renders ref; SVGs live at -// //.{pr,base}.svg (matches the bench job's +// Commit-pinned base blob URL (.../blob/) from publish-renders.cjs; SVGs +// live at //.{pr,base}.svg (matches the bench job's // `${traj%.traj}.${side}.svg` render paths). const rendersUrl = argv["renders-url"] ?? null; diff --git a/scripts/ci/publish-renders.cjs b/scripts/ci/publish-renders.cjs index a8d2ff50f1..2cbe8e5cde 100644 --- a/scripts/ci/publish-renders.cjs +++ b/scripts/ci/publish-renders.cjs @@ -1,4 +1,10 @@ -// push renders to orphan ref for viewing +// Stash the rendered SVGs as an orphan commit kept alive by a CUSTOM ref +// (`refs/bench-renders/`), NOT a branch. Refs outside refs/heads/* and +// refs/tags/* don't appear in GitHub's Branches or Tags UI, so per-run pushes +// don't pollute it — but the commit is still reachable, so it won't be GC'd and +// its blobs stay viewable in the web UI. Links use the commit SHA directly +// (GitHub's blob viewer resolves any SHA reachable from any ref), so we never +// need a branch/tag name in the URL. const fs = require("fs"); const path = require("path"); @@ -45,11 +51,11 @@ module.exports = async ({ github, context, core }) => { await github.rest.git.createRef({ owner, repo, - ref: `refs/heads/${refName}`, + ref: `refs/${refName}`, sha: c.data.sha, }); core.setOutput( "url", - `${process.env.GITHUB_SERVER_URL}/${owner}/${repo}/blob/${refName}`, + `${process.env.GITHUB_SERVER_URL}/${owner}/${repo}/blob/${c.data.sha}`, ); }; From 29e65759895011372cc64591805eb163cb643b62 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 01:23:23 -0400 Subject: [PATCH 15/53] clean, speed up CI --- .github/workflows/benchmark.yml | 30 ++++-------------------------- 1 file changed, 4 insertions(+), 26 deletions(-) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index bff7f12373..d386b3694e 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -40,7 +40,7 @@ jobs: (github.event.comment.author_association == 'OWNER' || github.event.comment.author_association == 'MEMBER' || github.event.comment.author_association == 'COLLABORATOR') - runs-on: ubuntu-24.04-arm + runs-on: ubuntu-slim outputs: head_sha: ${{ steps.pr.outputs.head_sha }} base_sha: ${{ steps.pr.outputs.base_sha }} @@ -73,8 +73,6 @@ jobs: repository: ${{ steps.pr.outputs.head_repo }} ref: ${{ steps.pr.outputs.head_sha }} persist-credentials: false - # resolve only needs the variant list, so stay sparse — but the - # enumerator now lives in a file, so that path has to be included too. sparse-checkout: | test-projects scripts/ci/enumerate-variants.sh @@ -117,20 +115,6 @@ jobs: sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 200 sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 200 - - name: Install Linux dependencies - run: | - sudo apt-get update -q - sudo apt-get install -y \ - build-essential \ - curl \ - file \ - libayatana-appindicator3-dev \ - librsvg2-dev \ - libssl-dev \ - libwebkit2gtk-4.1-dev \ - libxdo-dev \ - wget - - name: Set up sccache uses: mozilla-actions/sccache-action@v0.0.10 @@ -171,13 +155,6 @@ jobs: ref: ${{ needs.resolve.outputs.head_sha }} persist-credentials: false - - name: Install Linux dependencies - run: | - sudo apt-get update -q - sudo apt-get install -y \ - build-essential \ - libssl-dev - - name: Download PR binary uses: actions/download-artifact@v7 with: @@ -233,8 +210,7 @@ jobs: # -------------------------------------------------------------------------- collate: needs: [resolve, bench] - if: ${{ !cancelled() && needs.resolve.result == 'success' }} - runs-on: ubuntu-24.04-arm + runs-on: ubuntu-slim steps: - name: Checkout PR head uses: actions/checkout@v6 @@ -242,6 +218,8 @@ jobs: repository: ${{ needs.resolve.outputs.head_repo }} ref: ${{ needs.resolve.outputs.head_sha }} persist-credentials: false + sparse-checkout: scripts + sparse-checkout-cone-mode: false - name: Merge per-variant artifacts continue-on-error: true # if every bench job was skipped, still comment From 72ab3c547d50727e82b82e3bb93bfc890ebc8065 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 01:27:09 -0400 Subject: [PATCH 16/53] bench timeout --- scripts/run-bench.sh | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/scripts/run-bench.sh b/scripts/run-bench.sh index 0709c5ae5c..91e96b2363 100755 --- a/scripts/run-bench.sh +++ b/scripts/run-bench.sh @@ -43,11 +43,13 @@ for proj_dir in "${project_dirs[@]}"; do echo "--- $name run $run/$RUNS ---" # A per-trajectory solve failure is reported via ok:false in the JSON and # the CLI still exits 0; a nonzero exit means a hard failure (bad project, - # crash) — record it and keep going so other runs/variants still produce - # data instead of aborting the whole matrix job. - "$CLI" --chor "$chor" --all-trajectory --generate \ + # crash, or the 20m cap below) — record it and keep going so other + # runs/variants still produce data instead of aborting the whole matrix job. + # Cap each run at 20m so one runaway/hung solve can't burn the whole job's + # wall clock; -k 1m escalates to SIGKILL if the CLI ignores SIGTERM. + timeout -k 1m 20m "$CLI" --chor "$chor" --all-trajectory --generate \ --report-json "$OUT/$name.run$run.report.json" \ - || echo "choreo-cli exited nonzero on $name run $run/$RUNS" >&2 + || echo "choreo-cli failed or timed out (>20m) on $name run $run/$RUNS" >&2 done echo "::endgroup::" done From 260873f141a060c71c9aba26063ee178c84081ed Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 03:08:10 -0400 Subject: [PATCH 17/53] Revert "bench timeout" This reverts commit 2d4871d10821212ea806632fc094a02d6226f3ef. --- scripts/run-bench.sh | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/scripts/run-bench.sh b/scripts/run-bench.sh index 91e96b2363..0709c5ae5c 100755 --- a/scripts/run-bench.sh +++ b/scripts/run-bench.sh @@ -43,13 +43,11 @@ for proj_dir in "${project_dirs[@]}"; do echo "--- $name run $run/$RUNS ---" # A per-trajectory solve failure is reported via ok:false in the JSON and # the CLI still exits 0; a nonzero exit means a hard failure (bad project, - # crash, or the 20m cap below) — record it and keep going so other - # runs/variants still produce data instead of aborting the whole matrix job. - # Cap each run at 20m so one runaway/hung solve can't burn the whole job's - # wall clock; -k 1m escalates to SIGKILL if the CLI ignores SIGTERM. - timeout -k 1m 20m "$CLI" --chor "$chor" --all-trajectory --generate \ + # crash) — record it and keep going so other runs/variants still produce + # data instead of aborting the whole matrix job. + "$CLI" --chor "$chor" --all-trajectory --generate \ --report-json "$OUT/$name.run$run.report.json" \ - || echo "choreo-cli failed or timed out (>20m) on $name run $run/$RUNS" >&2 + || echo "choreo-cli exited nonzero on $name run $run/$RUNS" >&2 done echo "::endgroup::" done From e112704ac28a0b0c35e258966dfc3dc0349a8141 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 03:10:04 -0400 Subject: [PATCH 18/53] new bench timeouts --- .github/workflows/benchmark.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index d386b3694e..308b10ad4e 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -181,10 +181,14 @@ jobs: cp -r "test-projects/$VARIANT" "tp-base/$VARIANT" - name: Run PR benchmark + continue-on-error: true + timeout-minutes: 30 run: bash scripts/run-bench.sh "$PWD/bin-pr/choreo-cli" tp-pr out-pr "$VARIANT" + - name: Run base benchmark continue-on-error: true # tolerate missing / schema-incompatible base + timeout-minutes: 30 run: | if [ -x bin-base/choreo-cli ]; then bash scripts/run-bench.sh "$PWD/bin-base/choreo-cli" tp-base out-base "$VARIANT" From 81b1302da6557c1da254eaff40741a85442a8ff5 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 05:21:05 -0400 Subject: [PATCH 19/53] update CI report to clarify run cnt --- scripts/bench-report.mjs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index 1481539e35..59b696b48f 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -297,13 +297,11 @@ function fmtSolve(s) { if (!s || !s.ok || s.mean == null) return "—"; const u = unitFor(s.mean); const m = inUnit(s.mean, u).toFixed(decimals(u)); - const body = s.n > 1 && s.sd > 0 + const body = s.n > 1 ? `${m} ± ${inUnit(s.sd, u).toFixed(decimals(u))} ${u}` : `${m} ${u}`; - // Make a flaky side identifiable in the table itself (the summary only gives - // a count). Flaky = generated on some but not all runs; the mean is over the - // successful ones only. - return s.flaky ? `${body} ⚠ ${s.n}/${s.runs} ok` : body; + if (s.flaky) return `${body} ⚠ ${s.n}/${s.runs} ok`; + return s.n > 1 ? body : `${body} (1 run)`; } function fmtDeltaSolve(d, pct) { From 702edbd72be2ac7d2dd1e3d02b4798bbb9cf1154 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 05:46:32 -0400 Subject: [PATCH 20/53] way more test benches --- scripts/bench-local.sh | 13 +- .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-base-extra-low-dt/project.chor | 110 +++++++++++ .../swerve-base-extra-low-dt/small_curve.traj | 108 ++++++++++ .../straight_line.traj | 102 ++++++++++ .../swerve-base-extra-low-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 0 test-projects/swerve-base-low-dt/project.chor | 110 +++++++++++ .../small_curve.traj | 0 .../straight_line.traj | 0 .../sweep_auto.traj | 0 .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-base-medium-dt/project.chor | 110 +++++++++++ .../swerve-base-medium-dt/small_curve.traj | 108 ++++++++++ .../swerve-base-medium-dt/straight_line.traj | 102 ++++++++++ .../swerve-base-medium-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-fast-extra-low-dt/project.chor | 110 +++++++++++ .../swerve-fast-extra-low-dt/small_curve.traj | 108 ++++++++++ .../straight_line.traj | 102 ++++++++++ .../swerve-fast-extra-low-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-fast-high-dt/project.chor | 110 +++++++++++ .../swerve-fast-high-dt/small_curve.traj | 108 ++++++++++ .../swerve-fast-high-dt/straight_line.traj | 102 ++++++++++ .../swerve-fast-high-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 0 test-projects/swerve-fast-low-dt/project.chor | 110 +++++++++++ .../small_curve.traj | 0 .../straight_line.traj | 0 .../sweep_auto.traj | 0 .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-fast-medium-dt/project.chor | 110 +++++++++++ .../swerve-fast-medium-dt/small_curve.traj | 108 ++++++++++ .../swerve-fast-medium-dt/straight_line.traj | 102 ++++++++++ .../swerve-fast-medium-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-heavy-extra-low-dt/project.chor | 110 +++++++++++ .../small_curve.traj | 108 ++++++++++ .../straight_line.traj | 102 ++++++++++ .../swerve-heavy-extra-low-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-heavy-high-dt/project.chor | 110 +++++++++++ .../swerve-heavy-high-dt/small_curve.traj | 108 ++++++++++ .../swerve-heavy-high-dt/straight_line.traj | 102 ++++++++++ .../swerve-heavy-high-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 0 .../swerve-heavy-low-dt/project.chor | 110 +++++++++++ .../small_curve.traj | 0 .../straight_line.traj | 0 .../sweep_auto.traj | 0 .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-heavy-medium-dt/project.chor | 110 +++++++++++ .../swerve-heavy-medium-dt/small_curve.traj | 108 ++++++++++ .../swerve-heavy-medium-dt/straight_line.traj | 102 ++++++++++ .../swerve-heavy-medium-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-weak-extra-low-dt/project.chor | 110 +++++++++++ .../swerve-weak-extra-low-dt/small_curve.traj | 108 ++++++++++ .../straight_line.traj | 102 ++++++++++ .../swerve-weak-extra-low-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-weak-high-dt/project.chor | 110 +++++++++++ .../swerve-weak-high-dt/small_curve.traj | 108 ++++++++++ .../swerve-weak-high-dt/straight_line.traj | 102 ++++++++++ .../swerve-weak-high-dt/sweep_auto.traj | 157 +++++++++++++++ .../full_field_sweeper.traj | 0 test-projects/swerve-weak-low-dt/project.chor | 110 +++++++++++ .../small_curve.traj | 0 .../straight_line.traj | 0 .../sweep_auto.traj | 0 .../full_field_sweeper.traj | 184 ++++++++++++++++++ .../swerve-weak-medium-dt/project.chor | 110 +++++++++++ .../swerve-weak-medium-dt/small_curve.traj | 108 ++++++++++ .../swerve-weak-medium-dt/straight_line.traj | 102 ++++++++++ .../swerve-weak-medium-dt/sweep_auto.traj | 157 +++++++++++++++ 76 files changed, 7720 insertions(+), 4 deletions(-) create mode 100644 test-projects/swerve-base-extra-low-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-base-extra-low-dt/project.chor create mode 100644 test-projects/swerve-base-extra-low-dt/small_curve.traj create mode 100644 test-projects/swerve-base-extra-low-dt/straight_line.traj create mode 100644 test-projects/swerve-base-extra-low-dt/sweep_auto.traj rename test-projects/{swerve-base => swerve-base-low-dt}/full_field_sweeper.traj (100%) create mode 100755 test-projects/swerve-base-low-dt/project.chor rename test-projects/{swerve-base => swerve-base-low-dt}/small_curve.traj (100%) rename test-projects/{swerve-base => swerve-base-low-dt}/straight_line.traj (100%) rename test-projects/{swerve-base => swerve-base-low-dt}/sweep_auto.traj (100%) create mode 100644 test-projects/swerve-base-medium-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-base-medium-dt/project.chor create mode 100644 test-projects/swerve-base-medium-dt/small_curve.traj create mode 100644 test-projects/swerve-base-medium-dt/straight_line.traj create mode 100644 test-projects/swerve-base-medium-dt/sweep_auto.traj create mode 100644 test-projects/swerve-fast-extra-low-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-fast-extra-low-dt/project.chor create mode 100644 test-projects/swerve-fast-extra-low-dt/small_curve.traj create mode 100644 test-projects/swerve-fast-extra-low-dt/straight_line.traj create mode 100644 test-projects/swerve-fast-extra-low-dt/sweep_auto.traj create mode 100644 test-projects/swerve-fast-high-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-fast-high-dt/project.chor create mode 100644 test-projects/swerve-fast-high-dt/small_curve.traj create mode 100644 test-projects/swerve-fast-high-dt/straight_line.traj create mode 100644 test-projects/swerve-fast-high-dt/sweep_auto.traj rename test-projects/{swerve-fast => swerve-fast-low-dt}/full_field_sweeper.traj (100%) create mode 100755 test-projects/swerve-fast-low-dt/project.chor rename test-projects/{swerve-fast => swerve-fast-low-dt}/small_curve.traj (100%) rename test-projects/{swerve-fast => swerve-fast-low-dt}/straight_line.traj (100%) rename test-projects/{swerve-fast => swerve-fast-low-dt}/sweep_auto.traj (100%) create mode 100644 test-projects/swerve-fast-medium-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-fast-medium-dt/project.chor create mode 100644 test-projects/swerve-fast-medium-dt/small_curve.traj create mode 100644 test-projects/swerve-fast-medium-dt/straight_line.traj create mode 100644 test-projects/swerve-fast-medium-dt/sweep_auto.traj create mode 100644 test-projects/swerve-heavy-extra-low-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-heavy-extra-low-dt/project.chor create mode 100644 test-projects/swerve-heavy-extra-low-dt/small_curve.traj create mode 100644 test-projects/swerve-heavy-extra-low-dt/straight_line.traj create mode 100644 test-projects/swerve-heavy-extra-low-dt/sweep_auto.traj create mode 100644 test-projects/swerve-heavy-high-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-heavy-high-dt/project.chor create mode 100644 test-projects/swerve-heavy-high-dt/small_curve.traj create mode 100644 test-projects/swerve-heavy-high-dt/straight_line.traj create mode 100644 test-projects/swerve-heavy-high-dt/sweep_auto.traj rename test-projects/{swerve-heavy => swerve-heavy-low-dt}/full_field_sweeper.traj (100%) create mode 100755 test-projects/swerve-heavy-low-dt/project.chor rename test-projects/{swerve-heavy => swerve-heavy-low-dt}/small_curve.traj (100%) rename test-projects/{swerve-heavy => swerve-heavy-low-dt}/straight_line.traj (100%) rename test-projects/{swerve-heavy => swerve-heavy-low-dt}/sweep_auto.traj (100%) create mode 100644 test-projects/swerve-heavy-medium-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-heavy-medium-dt/project.chor create mode 100644 test-projects/swerve-heavy-medium-dt/small_curve.traj create mode 100644 test-projects/swerve-heavy-medium-dt/straight_line.traj create mode 100644 test-projects/swerve-heavy-medium-dt/sweep_auto.traj create mode 100644 test-projects/swerve-weak-extra-low-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-weak-extra-low-dt/project.chor create mode 100644 test-projects/swerve-weak-extra-low-dt/small_curve.traj create mode 100644 test-projects/swerve-weak-extra-low-dt/straight_line.traj create mode 100644 test-projects/swerve-weak-extra-low-dt/sweep_auto.traj create mode 100644 test-projects/swerve-weak-high-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-weak-high-dt/project.chor create mode 100644 test-projects/swerve-weak-high-dt/small_curve.traj create mode 100644 test-projects/swerve-weak-high-dt/straight_line.traj create mode 100644 test-projects/swerve-weak-high-dt/sweep_auto.traj rename test-projects/{swerve-weak => swerve-weak-low-dt}/full_field_sweeper.traj (100%) create mode 100755 test-projects/swerve-weak-low-dt/project.chor rename test-projects/{swerve-weak => swerve-weak-low-dt}/small_curve.traj (100%) rename test-projects/{swerve-weak => swerve-weak-low-dt}/straight_line.traj (100%) rename test-projects/{swerve-weak => swerve-weak-low-dt}/sweep_auto.traj (100%) create mode 100644 test-projects/swerve-weak-medium-dt/full_field_sweeper.traj create mode 100755 test-projects/swerve-weak-medium-dt/project.chor create mode 100644 test-projects/swerve-weak-medium-dt/small_curve.traj create mode 100644 test-projects/swerve-weak-medium-dt/straight_line.traj create mode 100644 test-projects/swerve-weak-medium-dt/sweep_auto.traj diff --git a/scripts/bench-local.sh b/scripts/bench-local.sh index 342e0e7fc1..e4aa71a8b3 100755 --- a/scripts/bench-local.sh +++ b/scripts/bench-local.sh @@ -7,8 +7,9 @@ # scripts/bench-local.sh # PR-only run, no comparison # scripts/bench-local.sh # full comparison vs (e.g. main) # -# Requires: cargo, node, hyperfine. The base build uses `git worktree` so your -# current checkout stays untouched. +# Requires: cargo, node, and `timeout` (GNU coreutils — run-bench.sh caps each +# run at 20m). The base build uses `git worktree` so your current checkout stays +# untouched. set -euo pipefail @@ -27,7 +28,7 @@ if [ ! -d "$REPO_ROOT/test-projects" ]; then echo "test-projects/ not found at repo root" >&2 exit 1 fi -for cmd in cargo node hyperfine; do +for cmd in cargo node timeout; do command -v "$cmd" >/dev/null || { echo "missing required command: $cmd" >&2; exit 1; } done @@ -84,10 +85,12 @@ render_side() { [ -f "$traj" ] || continue variant=$(basename "$(dirname "$traj")") name=$(basename "$traj" .traj) + # Best-effort per trajectory, matching scripts/ci/render-variant.sh: one + # bad render must not abort the whole local run under `set -e`. node "$REPO_ROOT/scripts/render-traj.mjs" \ --traj "$traj" \ --out "${traj%.traj}.${side}.svg" \ - --title "${side^^} ${variant} · ${name}" + --title "${side^^} ${variant} · ${name}" || true done } render_side pr "$PR_TP" @@ -103,6 +106,8 @@ find "$PR_TP" "$BASE_TP" -name "*.svg" -print0 2>/dev/null | while IFS= read -r done echo "==> Building markdown report" +# No --commit: a local run benches the working tree (typically with uncommitted +# changes), so a HEAD SHA in the report header would mislabel what was built. node "$REPO_ROOT/scripts/bench-report.mjs" \ --pr "$PR_TP" \ --base "$BASE_TP" \ diff --git a/test-projects/swerve-base-extra-low-dt/full_field_sweeper.traj b/test-projects/swerve-base-extra-low-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..ec4011c503 --- /dev/null +++ b/test-projects/swerve-base-extra-low-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-extra-low-dt/project.chor b/test-projects/swerve-base-extra-low-dt/project.chor new file mode 100755 index 0000000000..1a831c724e --- /dev/null +++ b/test-projects/swerve-base-extra-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-base-extra-low-dt/small_curve.traj b/test-projects/swerve-base-extra-low-dt/small_curve.traj new file mode 100644 index 0000000000..751ff3756b --- /dev/null +++ b/test-projects/swerve-base-extra-low-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-extra-low-dt/straight_line.traj b/test-projects/swerve-base-extra-low-dt/straight_line.traj new file mode 100644 index 0000000000..08ce6a1d0d --- /dev/null +++ b/test-projects/swerve-base-extra-low-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-extra-low-dt/sweep_auto.traj b/test-projects/swerve-base-extra-low-dt/sweep_auto.traj new file mode 100644 index 0000000000..0cd6636286 --- /dev/null +++ b/test-projects/swerve-base-extra-low-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base/full_field_sweeper.traj b/test-projects/swerve-base-low-dt/full_field_sweeper.traj similarity index 100% rename from test-projects/swerve-base/full_field_sweeper.traj rename to test-projects/swerve-base-low-dt/full_field_sweeper.traj diff --git a/test-projects/swerve-base-low-dt/project.chor b/test-projects/swerve-base-low-dt/project.chor new file mode 100755 index 0000000000..1a831c724e --- /dev/null +++ b/test-projects/swerve-base-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-base/small_curve.traj b/test-projects/swerve-base-low-dt/small_curve.traj similarity index 100% rename from test-projects/swerve-base/small_curve.traj rename to test-projects/swerve-base-low-dt/small_curve.traj diff --git a/test-projects/swerve-base/straight_line.traj b/test-projects/swerve-base-low-dt/straight_line.traj similarity index 100% rename from test-projects/swerve-base/straight_line.traj rename to test-projects/swerve-base-low-dt/straight_line.traj diff --git a/test-projects/swerve-base/sweep_auto.traj b/test-projects/swerve-base-low-dt/sweep_auto.traj similarity index 100% rename from test-projects/swerve-base/sweep_auto.traj rename to test-projects/swerve-base-low-dt/sweep_auto.traj diff --git a/test-projects/swerve-base-medium-dt/full_field_sweeper.traj b/test-projects/swerve-base-medium-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..66abecb972 --- /dev/null +++ b/test-projects/swerve-base-medium-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-medium-dt/project.chor b/test-projects/swerve-base-medium-dt/project.chor new file mode 100755 index 0000000000..1a831c724e --- /dev/null +++ b/test-projects/swerve-base-medium-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-base-medium-dt/small_curve.traj b/test-projects/swerve-base-medium-dt/small_curve.traj new file mode 100644 index 0000000000..b80bf50d68 --- /dev/null +++ b/test-projects/swerve-base-medium-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-medium-dt/straight_line.traj b/test-projects/swerve-base-medium-dt/straight_line.traj new file mode 100644 index 0000000000..47955d6da4 --- /dev/null +++ b/test-projects/swerve-base-medium-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-base-medium-dt/sweep_auto.traj b/test-projects/swerve-base-medium-dt/sweep_auto.traj new file mode 100644 index 0000000000..59bb291fed --- /dev/null +++ b/test-projects/swerve-base-medium-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-extra-low-dt/full_field_sweeper.traj b/test-projects/swerve-fast-extra-low-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..ec4011c503 --- /dev/null +++ b/test-projects/swerve-fast-extra-low-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-extra-low-dt/project.chor b/test-projects/swerve-fast-extra-low-dt/project.chor new file mode 100755 index 0000000000..1efc6d2281 --- /dev/null +++ b/test-projects/swerve-fast-extra-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"5.27", + "val":5.27 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-fast-extra-low-dt/small_curve.traj b/test-projects/swerve-fast-extra-low-dt/small_curve.traj new file mode 100644 index 0000000000..751ff3756b --- /dev/null +++ b/test-projects/swerve-fast-extra-low-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-extra-low-dt/straight_line.traj b/test-projects/swerve-fast-extra-low-dt/straight_line.traj new file mode 100644 index 0000000000..08ce6a1d0d --- /dev/null +++ b/test-projects/swerve-fast-extra-low-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-extra-low-dt/sweep_auto.traj b/test-projects/swerve-fast-extra-low-dt/sweep_auto.traj new file mode 100644 index 0000000000..0cd6636286 --- /dev/null +++ b/test-projects/swerve-fast-extra-low-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-high-dt/full_field_sweeper.traj b/test-projects/swerve-fast-high-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..d772fc87d1 --- /dev/null +++ b/test-projects/swerve-fast-high-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-high-dt/project.chor b/test-projects/swerve-fast-high-dt/project.chor new file mode 100755 index 0000000000..1efc6d2281 --- /dev/null +++ b/test-projects/swerve-fast-high-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"5.27", + "val":5.27 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-fast-high-dt/small_curve.traj b/test-projects/swerve-fast-high-dt/small_curve.traj new file mode 100644 index 0000000000..9ffc61a53b --- /dev/null +++ b/test-projects/swerve-fast-high-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-high-dt/straight_line.traj b/test-projects/swerve-fast-high-dt/straight_line.traj new file mode 100644 index 0000000000..41e4360d5c --- /dev/null +++ b/test-projects/swerve-fast-high-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-high-dt/sweep_auto.traj b/test-projects/swerve-fast-high-dt/sweep_auto.traj new file mode 100644 index 0000000000..2942f1ac8b --- /dev/null +++ b/test-projects/swerve-fast-high-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast/full_field_sweeper.traj b/test-projects/swerve-fast-low-dt/full_field_sweeper.traj similarity index 100% rename from test-projects/swerve-fast/full_field_sweeper.traj rename to test-projects/swerve-fast-low-dt/full_field_sweeper.traj diff --git a/test-projects/swerve-fast-low-dt/project.chor b/test-projects/swerve-fast-low-dt/project.chor new file mode 100755 index 0000000000..1efc6d2281 --- /dev/null +++ b/test-projects/swerve-fast-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"5.27", + "val":5.27 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-fast/small_curve.traj b/test-projects/swerve-fast-low-dt/small_curve.traj similarity index 100% rename from test-projects/swerve-fast/small_curve.traj rename to test-projects/swerve-fast-low-dt/small_curve.traj diff --git a/test-projects/swerve-fast/straight_line.traj b/test-projects/swerve-fast-low-dt/straight_line.traj similarity index 100% rename from test-projects/swerve-fast/straight_line.traj rename to test-projects/swerve-fast-low-dt/straight_line.traj diff --git a/test-projects/swerve-fast/sweep_auto.traj b/test-projects/swerve-fast-low-dt/sweep_auto.traj similarity index 100% rename from test-projects/swerve-fast/sweep_auto.traj rename to test-projects/swerve-fast-low-dt/sweep_auto.traj diff --git a/test-projects/swerve-fast-medium-dt/full_field_sweeper.traj b/test-projects/swerve-fast-medium-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..66abecb972 --- /dev/null +++ b/test-projects/swerve-fast-medium-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-medium-dt/project.chor b/test-projects/swerve-fast-medium-dt/project.chor new file mode 100755 index 0000000000..1efc6d2281 --- /dev/null +++ b/test-projects/swerve-fast-medium-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"5.27", + "val":5.27 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-fast-medium-dt/small_curve.traj b/test-projects/swerve-fast-medium-dt/small_curve.traj new file mode 100644 index 0000000000..b80bf50d68 --- /dev/null +++ b/test-projects/swerve-fast-medium-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-medium-dt/straight_line.traj b/test-projects/swerve-fast-medium-dt/straight_line.traj new file mode 100644 index 0000000000..47955d6da4 --- /dev/null +++ b/test-projects/swerve-fast-medium-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-fast-medium-dt/sweep_auto.traj b/test-projects/swerve-fast-medium-dt/sweep_auto.traj new file mode 100644 index 0000000000..59bb291fed --- /dev/null +++ b/test-projects/swerve-fast-medium-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-extra-low-dt/full_field_sweeper.traj b/test-projects/swerve-heavy-extra-low-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..ec4011c503 --- /dev/null +++ b/test-projects/swerve-heavy-extra-low-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-extra-low-dt/project.chor b/test-projects/swerve-heavy-extra-low-dt/project.chor new file mode 100755 index 0000000000..723e3c00d2 --- /dev/null +++ b/test-projects/swerve-heavy-extra-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-heavy-extra-low-dt/small_curve.traj b/test-projects/swerve-heavy-extra-low-dt/small_curve.traj new file mode 100644 index 0000000000..751ff3756b --- /dev/null +++ b/test-projects/swerve-heavy-extra-low-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-extra-low-dt/straight_line.traj b/test-projects/swerve-heavy-extra-low-dt/straight_line.traj new file mode 100644 index 0000000000..08ce6a1d0d --- /dev/null +++ b/test-projects/swerve-heavy-extra-low-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-extra-low-dt/sweep_auto.traj b/test-projects/swerve-heavy-extra-low-dt/sweep_auto.traj new file mode 100644 index 0000000000..0cd6636286 --- /dev/null +++ b/test-projects/swerve-heavy-extra-low-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-high-dt/full_field_sweeper.traj b/test-projects/swerve-heavy-high-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..d772fc87d1 --- /dev/null +++ b/test-projects/swerve-heavy-high-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-high-dt/project.chor b/test-projects/swerve-heavy-high-dt/project.chor new file mode 100755 index 0000000000..723e3c00d2 --- /dev/null +++ b/test-projects/swerve-heavy-high-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-heavy-high-dt/small_curve.traj b/test-projects/swerve-heavy-high-dt/small_curve.traj new file mode 100644 index 0000000000..9ffc61a53b --- /dev/null +++ b/test-projects/swerve-heavy-high-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-high-dt/straight_line.traj b/test-projects/swerve-heavy-high-dt/straight_line.traj new file mode 100644 index 0000000000..41e4360d5c --- /dev/null +++ b/test-projects/swerve-heavy-high-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-high-dt/sweep_auto.traj b/test-projects/swerve-heavy-high-dt/sweep_auto.traj new file mode 100644 index 0000000000..2942f1ac8b --- /dev/null +++ b/test-projects/swerve-heavy-high-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy/full_field_sweeper.traj b/test-projects/swerve-heavy-low-dt/full_field_sweeper.traj similarity index 100% rename from test-projects/swerve-heavy/full_field_sweeper.traj rename to test-projects/swerve-heavy-low-dt/full_field_sweeper.traj diff --git a/test-projects/swerve-heavy-low-dt/project.chor b/test-projects/swerve-heavy-low-dt/project.chor new file mode 100755 index 0000000000..723e3c00d2 --- /dev/null +++ b/test-projects/swerve-heavy-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-heavy/small_curve.traj b/test-projects/swerve-heavy-low-dt/small_curve.traj similarity index 100% rename from test-projects/swerve-heavy/small_curve.traj rename to test-projects/swerve-heavy-low-dt/small_curve.traj diff --git a/test-projects/swerve-heavy/straight_line.traj b/test-projects/swerve-heavy-low-dt/straight_line.traj similarity index 100% rename from test-projects/swerve-heavy/straight_line.traj rename to test-projects/swerve-heavy-low-dt/straight_line.traj diff --git a/test-projects/swerve-heavy/sweep_auto.traj b/test-projects/swerve-heavy-low-dt/sweep_auto.traj similarity index 100% rename from test-projects/swerve-heavy/sweep_auto.traj rename to test-projects/swerve-heavy-low-dt/sweep_auto.traj diff --git a/test-projects/swerve-heavy-medium-dt/full_field_sweeper.traj b/test-projects/swerve-heavy-medium-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..66abecb972 --- /dev/null +++ b/test-projects/swerve-heavy-medium-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-medium-dt/project.chor b/test-projects/swerve-heavy-medium-dt/project.chor new file mode 100755 index 0000000000..723e3c00d2 --- /dev/null +++ b/test-projects/swerve-heavy-medium-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"2", + "val":2.0 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5784 RPM", + "val":605.6990636121121 + }, + "stall_torque":{ + "exp":"9.36 N * m", + "val":9.36 + }, + "kT":{ + "exp":"0.0197 N * m / A", + "val":0.0197 + }, + "kV":{ + "exp":"0.0020593 V / rpm", + "val":0.019664866458548407 + }, + "supply_limit":{ + "exp":"60 A", + "val":60.0 + }, + "stator_limit":{ + "exp":"120 A", + "val":120.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-heavy-medium-dt/small_curve.traj b/test-projects/swerve-heavy-medium-dt/small_curve.traj new file mode 100644 index 0000000000..b80bf50d68 --- /dev/null +++ b/test-projects/swerve-heavy-medium-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-medium-dt/straight_line.traj b/test-projects/swerve-heavy-medium-dt/straight_line.traj new file mode 100644 index 0000000000..47955d6da4 --- /dev/null +++ b/test-projects/swerve-heavy-medium-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-heavy-medium-dt/sweep_auto.traj b/test-projects/swerve-heavy-medium-dt/sweep_auto.traj new file mode 100644 index 0000000000..59bb291fed --- /dev/null +++ b/test-projects/swerve-heavy-medium-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-extra-low-dt/full_field_sweeper.traj b/test-projects/swerve-weak-extra-low-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..ec4011c503 --- /dev/null +++ b/test-projects/swerve-weak-extra-low-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-extra-low-dt/project.chor b/test-projects/swerve-weak-extra-low-dt/project.chor new file mode 100755 index 0000000000..14e63be164 --- /dev/null +++ b/test-projects/swerve-weak-extra-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "stall_torque":{ + "exp":"4.2 N * m", + "val":4.2 + }, + "kT":{ + "exp":"0.0194 N * m / A", + "val":0.0194 + }, + "kV":{ + "exp":"0.002014 V / RPM", + "val":0.019232283323224637 + }, + "supply_limit":{ + "exp":"40 A", + "val":40.0 + }, + "stator_limit":{ + "exp":"40 A", + "val":40.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-weak-extra-low-dt/small_curve.traj b/test-projects/swerve-weak-extra-low-dt/small_curve.traj new file mode 100644 index 0000000000..751ff3756b --- /dev/null +++ b/test-projects/swerve-weak-extra-low-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-extra-low-dt/straight_line.traj b/test-projects/swerve-weak-extra-low-dt/straight_line.traj new file mode 100644 index 0000000000..08ce6a1d0d --- /dev/null +++ b/test-projects/swerve-weak-extra-low-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-extra-low-dt/sweep_auto.traj b/test-projects/swerve-weak-extra-low-dt/sweep_auto.traj new file mode 100644 index 0000000000..0cd6636286 --- /dev/null +++ b/test-projects/swerve-weak-extra-low-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.01 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.01 s", + "val":0.01 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-high-dt/full_field_sweeper.traj b/test-projects/swerve-weak-high-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..d772fc87d1 --- /dev/null +++ b/test-projects/swerve-weak-high-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-high-dt/project.chor b/test-projects/swerve-weak-high-dt/project.chor new file mode 100755 index 0000000000..14e63be164 --- /dev/null +++ b/test-projects/swerve-weak-high-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "stall_torque":{ + "exp":"4.2 N * m", + "val":4.2 + }, + "kT":{ + "exp":"0.0194 N * m / A", + "val":0.0194 + }, + "kV":{ + "exp":"0.002014 V / RPM", + "val":0.019232283323224637 + }, + "supply_limit":{ + "exp":"40 A", + "val":40.0 + }, + "stator_limit":{ + "exp":"40 A", + "val":40.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-weak-high-dt/small_curve.traj b/test-projects/swerve-weak-high-dt/small_curve.traj new file mode 100644 index 0000000000..9ffc61a53b --- /dev/null +++ b/test-projects/swerve-weak-high-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-high-dt/straight_line.traj b/test-projects/swerve-weak-high-dt/straight_line.traj new file mode 100644 index 0000000000..41e4360d5c --- /dev/null +++ b/test-projects/swerve-weak-high-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-high-dt/sweep_auto.traj b/test-projects/swerve-weak-high-dt/sweep_auto.traj new file mode 100644 index 0000000000..2942f1ac8b --- /dev/null +++ b/test-projects/swerve-weak-high-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.1 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.1 s", + "val":0.1 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak/full_field_sweeper.traj b/test-projects/swerve-weak-low-dt/full_field_sweeper.traj similarity index 100% rename from test-projects/swerve-weak/full_field_sweeper.traj rename to test-projects/swerve-weak-low-dt/full_field_sweeper.traj diff --git a/test-projects/swerve-weak-low-dt/project.chor b/test-projects/swerve-weak-low-dt/project.chor new file mode 100755 index 0000000000..14e63be164 --- /dev/null +++ b/test-projects/swerve-weak-low-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "stall_torque":{ + "exp":"4.2 N * m", + "val":4.2 + }, + "kT":{ + "exp":"0.0194 N * m / A", + "val":0.0194 + }, + "kV":{ + "exp":"0.002014 V / RPM", + "val":0.019232283323224637 + }, + "supply_limit":{ + "exp":"40 A", + "val":40.0 + }, + "stator_limit":{ + "exp":"40 A", + "val":40.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-weak/small_curve.traj b/test-projects/swerve-weak-low-dt/small_curve.traj similarity index 100% rename from test-projects/swerve-weak/small_curve.traj rename to test-projects/swerve-weak-low-dt/small_curve.traj diff --git a/test-projects/swerve-weak/straight_line.traj b/test-projects/swerve-weak-low-dt/straight_line.traj similarity index 100% rename from test-projects/swerve-weak/straight_line.traj rename to test-projects/swerve-weak-low-dt/straight_line.traj diff --git a/test-projects/swerve-weak/sweep_auto.traj b/test-projects/swerve-weak-low-dt/sweep_auto.traj similarity index 100% rename from test-projects/swerve-weak/sweep_auto.traj rename to test-projects/swerve-weak-low-dt/sweep_auto.traj diff --git a/test-projects/swerve-weak-medium-dt/full_field_sweeper.traj b/test-projects/swerve-weak-medium-dt/full_field_sweeper.traj new file mode 100644 index 0000000000..66abecb972 --- /dev/null +++ b/test-projects/swerve-weak-medium-dt/full_field_sweeper.traj @@ -0,0 +1,184 @@ +{ + "name":"full_field_sweeper", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.42465028166770935, "y":1.369949221611023, "heading":-0.7429850162326409, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":15.383139610290527, "y":1.4184211492538452, "heading":1.764755735877346, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":12.818838119506836, "y":3.626879930496216, "heading":-2.173634786129365, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.896666526794434, "y":5.656417369842529, "heading":-1.144169013902648, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8366799354553223, "y":2.8109304904937744, "heading":1.6672699934650728, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.575202941894531, "y":2.5033862590789795, "heading":1.6567910528072471, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.42465028166770935 m", "val":0.42465028166770935}, "y":{"exp":"1.369949221611023 m", "val":1.369949221611023}, "heading":{"exp":"-0.7429850162326409 rad", "val":-0.7429850162326409}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"15.383139610290527 m", "val":15.383139610290527}, "y":{"exp":"1.4184211492538452 m", "val":1.4184211492538452}, "heading":{"exp":"1.764755735877346 rad", "val":1.764755735877346}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"12.818838119506836 m", "val":12.818838119506836}, "y":{"exp":"3.626879930496216 m", "val":3.626879930496216}, "heading":{"exp":"-2.173634786129365 rad", "val":-2.173634786129365}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.896666526794434 m", "val":5.896666526794434}, "y":{"exp":"5.656417369842529 m", "val":5.656417369842529}, "heading":{"exp":"-1.144169013902648 rad", "val":-1.144169013902648}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8366799354553223 m", "val":2.8366799354553223}, "y":{"exp":"2.8109304904937744 m", "val":2.8109304904937744}, "heading":{"exp":"1.6672699934650728 rad", "val":1.6672699934650728}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.575202941894531 m", "val":9.575202941894531}, "y":{"exp":"2.5033862590789795 m", "val":2.5033862590789795}, "heading":{"exp":"1.6567910528072471 rad", "val":1.6567910528072471}, "intervals":947, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.65936,4.62129,6.24967,7.43568,9.24825], + "samples":[ + {"t":0.0, "x":0.42465, "y":1.36995, "heading":-0.74299, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.076, "ay":-0.3201, "alpha":8.05674, "fx":[240.12241,249.21901,237.64177,230.73199], "fy":[69.22141,-17.55823,-71.17241,-2.26969]}, + {"t":0.09383, "x":0.48661, "y":1.36854, "heading":-0.70752, "vx":1.32075, "vy":-0.03003, "omega":0.75596, "ax":10.65696, "ay":-0.39055, "alpha":4.0205, "fx":[180.4522,211.20154,169.37586,164.05777], "fy":[51.36281,-21.6037,-56.20552,-0.12641]}, + {"t":0.18766, "x":0.65745, "y":1.364, "heading":-0.61889, "vx":2.32069, "vy":-0.06668, "omega":1.1332, "ax":8.5826, "ay":-0.28734, "alpha":1.70482, "fx":[149.51048,168.50606,136.14002,129.7934], "fy":[30.33199,-17.78569,-33.63111,1.53469]}, + {"t":0.28149, "x":0.91298, "y":1.35648, "heading":-0.50506, "vx":3.12599, "vy":-0.09364, "omega":1.29317, "ax":7.04821, "ay":-0.24103, "alpha":-1.51848, "fx":[129.16586,137.76546,116.72217,95.89846], "fy":[6.1508,-9.51842,-12.66842,-0.36339]}, + {"t":0.37532, "x":1.23732, "y":1.34663, "heading":-0.3904, "vx":3.78732, "vy":-0.11626, "omega":1.15069, "ax":4.67976, "ay":-0.0785, "alpha":-5.18286, "fx":[113.23046,116.69098,56.20294,32.28141], "fy":[11.15199,-9.93744,-7.30915,0.75354]}, + {"t":0.46915, "x":1.61328, "y":1.33538, "heading":-0.30525, "vx":4.22642, "vy":-0.12362, "omega":0.66438, "ax":2.12022, "ay":-0.10853, "alpha":-2.66069, "fx":[54.62744,67.59591,16.95702,5.07668], "fy":[19.93274,-23.53672,-4.18395,0.40389]}, + {"t":0.56298, "x":2.01918, "y":1.3233, "heading":-0.25462, "vx":4.42536, "vy":-0.13381, "omega":0.41473, "ax":0.93662, "ay":-0.03826, "alpha":-1.825, "fx":[27.6959,34.01237,4.16874,-2.1504], "fy":[9.83394,-11.76184,-0.94364,0.26852]}, + {"t":0.65681, "x":2.43853, "y":1.31058, "heading":-0.22374, "vx":4.51325, "vy":-0.1374, "omega":0.24349, "ax":0.41007, "ay":-0.00955, "alpha":-1.09072, "fx":[14.00352,17.07629,0.17705,-3.35591], "fy":[5.24426,-6.32897,-0.03905,0.47371]}, + {"t":0.75064, "x":2.86382, "y":1.29765, "heading":-0.2057, "vx":4.55172, "vy":-0.13829, "omega":0.14115, "ax":0.16726, "ay":-0.00853, "alpha":-0.5513, "fx":[7.30589,8.52114,-1.01547,-3.4315], "fy":[3.75389,-4.71642,-0.18301,0.56489]}, + {"t":0.84447, "x":3.29164, "y":1.28463, "heading":-0.19488, "vx":4.56742, "vy":-0.13909, "omega":0.08942, "ax":0.06889, "ay":-0.00635, "alpha":-0.30521, "fx":[4.28952,4.58317,-1.22969,-2.95611], "fy":[2.73902,-3.45746,-0.24617,0.53223]}, + {"t":0.9383, "x":3.7205, "y":1.27155, "heading":-0.18783, "vx":4.57388, "vy":-0.13969, "omega":0.06078, "ax":0.03382, "ay":-0.00309, "alpha":-0.19781, "fx":[2.78786,2.77793,-1.06608,-2.1989], "fy":[1.90072,-2.30315,-0.21878,0.41063]}, + {"t":1.03213, "x":4.14982, "y":1.25843, "heading":-0.183, "vx":4.57705, "vy":-0.13998, "omega":0.04222, "ax":0.02064, "ay":-0.00023, "alpha":-0.13822, "fx":[1.87725,1.7782,-0.7601,-1.49082], "fy":[1.23865,-1.3816,-0.15351,0.28093]}, + {"t":1.12596, "x":4.57937, "y":1.2453, "heading":-0.17965, "vx":4.57899, "vy":-0.14, "omega":0.02925, "ax":0.01357, "ay":0.00141, "alpha":-0.09558, "fx":[1.2589,1.10359,-0.48847,-0.95092], "fy":[0.76234,-0.74909,-0.09489,0.1778]}, + {"t":1.21979, "x":5.00908, "y":1.23217, "heading":-0.17732, "vx":4.58026, "vy":-0.13987, "omega":0.02028, "ax":0.00911, "ay":0.00192, "alpha":-0.06407, "fx":[0.83746,0.64868,-0.28826,-0.57793], "fy":[0.44392,-0.36628,-0.05252,0.10539]}, + {"t":1.31362, "x":5.43888, "y":1.21905, "heading":-0.1757, "vx":4.58112, "vy":-0.13969, "omega":0.01427, "ax":-0.00219, "ay":0.00149, "alpha":-0.00612, "fx":[0.75096,-0.34672,-0.17128,-0.38213], "fy":[0.31616,-0.25291,-0.02748,0.06565]}, + {"t":1.40745, "x":5.86872, "y":1.20595, "heading":-0.17439, "vx":4.58091, "vy":-0.13955, "omega":0.0137, "ax":0.00649, "ay":0.00209, "alpha":-0.0332, "fx":[0.53599,0.29854,-0.11129,-0.2818], "fy":[0.25951,-0.14838,-0.01928,0.05069]}, + {"t":1.50128, "x":6.29857, "y":1.19287, "heading":-0.17325, "vx":4.58152, "vy":-0.13935, "omega":0.01058, "ax":-0.00135, "ay":0.00109, "alpha":0.00113, "fx":[0.51185,-0.30621,-0.07663,-0.22061], "fy":[0.20243,-0.15364,-0.01189,0.03752]}, + {"t":1.59511, "x":6.72845, "y":1.1798, "heading":-0.17225, "vx":4.5814, "vy":-0.13925, "omega":0.01069, "ax":-0.00074, "ay":0.00115, "alpha":0.00263, "fx":[0.53625,-0.31454,-0.06259,-0.20943], "fy":[0.21523,-0.16304,-0.00982,0.0359]}, + {"t":1.68894, "x":7.15832, "y":1.16674, "heading":-0.17124, "vx":4.58133, "vy":-0.13914, "omega":0.01094, "ax":-0.00153, "ay":0.00062, "alpha":0.00806, "fx":[0.57357,-0.39018,-0.06617,-0.2214], "fy":[0.23716,-0.22312,-0.01046,0.03842]}, + {"t":1.78277, "x":7.58818, "y":1.15368, "heading":-0.17018, "vx":4.58118, "vy":-0.13908, "omega":0.01169, "ax":-0.00516, "ay":-0.002, "alpha":0.02576, "fx":[0.67078,-0.63901,-0.09958,-0.28303], "fy":[0.29059,-0.46082,-0.01577,0.04997]}, + {"t":1.8766, "x":8.01801, "y":1.14062, "heading":-0.16897, "vx":4.5807, "vy":-0.13927, "omega":0.01411, "ax":0.00616, "ay":0.00308, "alpha":-0.02571, "fx":[0.54652,0.24286,-0.09876,-0.27161], "fy":[0.32876,-0.1534,-0.01844,0.05255]}, + {"t":1.97043, "x":8.44784, "y":1.12757, "heading":-0.16776, "vx":4.58128, "vy":-0.13898, "omega":0.0117, "ax":0.00431, "ay":0.00289, "alpha":-0.01797, "fx":[0.42731,0.12872,-0.06299,-0.20009], "fy":[0.24792,-0.07793,-0.01176,0.03848]}, + {"t":2.06426, "x":8.87772, "y":1.11454, "heading":-0.16674, "vx":4.58168, "vy":-0.13871, "omega":0.01001, "ax":-0.02046, "ay":0.02811, "alpha":0.1126, "fx":[-1.44997,0.69456,-0.18691,-0.44978], "fy":[2.09761,-0.23456,-0.02568,0.07549]}, + {"t":2.15808, "x":9.30753, "y":1.10165, "heading":-0.1653, "vx":4.57976, "vy":-0.13607, "omega":0.02057, "ax":0.01211, "ay":0.00281, "alpha":-0.04026, "fx":[0.89281,0.63795,-0.2155,-0.49152], "fy":[0.6718,-0.53736,-0.04292,0.09968]}, + {"t":2.25191, "x":9.7373, "y":1.0889, "heading":-0.16355, "vx":4.5809, "vy":-0.13581, "omega":0.0168, "ax":-0.02504, "ay":0.04653, "alpha":0.15159, "fx":[-1.96121,1.5298,-0.40767,-0.8645], "fy":[3.58786,-0.51124,-0.05508,0.14457]}, + {"t":2.34574, "x":10.16701, "y":1.07636, "heading":-0.16131, "vx":4.57855, "vy":-0.13144, "omega":0.03102, "ax":-0.02366, "ay":0.0704, "alpha":0.17304, "fx":[-2.43361,3.12599,-0.77173,-1.53053], "fy":[5.66914,-1.03158,-0.1024,0.25458]}, + {"t":2.43957, "x":10.59651, "y":1.06433, "heading":-0.15763, "vx":4.57633, "vy":-0.12484, "omega":0.04726, "ax":-0.02638, "ay":0.0994, "alpha":0.20275, "fx":[-3.08785,4.88831,-1.2546,-2.34041], "fy":[8.11539,-1.5764,-0.16241,0.38666]}, + {"t":2.5334, "x":11.02579, "y":1.05306, "heading":-0.15231, "vx":4.57385, "vy":-0.11551, "omega":0.06628, "ax":-0.03133, "ay":0.13535, "alpha":0.24101, "fx":[-3.94401,6.95832,-1.84937,-3.29631], "fy":[11.05119,-2.15159,-0.23044,0.54001]}, + {"t":2.62723, "x":11.45482, "y":1.04282, "heading":-0.14503, "vx":4.57091, "vy":-0.10281, "omega":0.0889, "ax":-0.03861, "ay":0.18131, "alpha":0.29077, "fx":[-5.08095,9.45842,-2.58015,-4.42404], "fy":[14.65263,-2.73042,-0.30361,0.7175]}, + {"t":2.72106, "x":11.88353, "y":1.03397, "heading":-0.13541, "vx":4.56729, "vy":-0.0858, "omega":0.11618, "ax":-0.04923, "ay":0.24208, "alpha":0.35714, "fx":[-6.63626,12.5427,-3.49057,-5.76558], "fy":[19.17697,-3.25228,-0.37746,0.92392]}, + {"t":2.81489, "x":12.31187, "y":1.02698, "heading":-0.12293, "vx":4.56267, "vy":-0.06308, "omega":0.14969, "ax":-0.0652, "ay":0.32526, "alpha":0.4481, "fx":[-8.83327,16.42428,-4.64629,-7.38088], "fy":[25.00807,-3.60038,-0.44301,1.16542]}, + {"t":2.90872, "x":12.73969, "y":1.0225, "heading":-0.10691, "vx":4.55655, "vy":-0.03256, "omega":0.19173, "ax":-0.08989, "ay":0.4427, "alpha":0.57483, "fx":[-12.02083,21.39729,-6.14214,-9.35026], "fy":[32.70645,-3.55309,-0.48141,1.44885]}, + {"t":3.00255, "x":13.16684, "y":1.02139, "heading":-0.08639, "vx":4.54812, "vy":0.00897, "omega":0.24567, "ax":-0.12733, "ay":0.61117, "alpha":0.74352, "fx":[-16.64085,27.82905,-8.09875,-11.75303], "fy":[42.94343,-2.68855,-0.45129,1.77994]}, + {"t":3.09638, "x":13.59303, "y":1.02492, "heading":-0.06007, "vx":4.53617, "vy":0.06632, "omega":0.31543, "ax":-0.17112, "ay":0.84105, "alpha":0.89393, "fx":[-22.54917,35.96763,-10.55891,-14.50261], "fy":[55.57593,-0.25175,-0.26362,2.16362]}, + {"t":3.19021, "x":14.0179, "y":1.03485, "heading":-0.02654, "vx":4.52012, "vy":0.14524, "omega":0.39931, "ax":-2.14997, "ay":2.83918, "alpha":9.17789, "fx":[-129.28559,44.06108,-26.03904,-35.01807], "fy":[181.70761,7.98102,2.16089,1.32475]}, + {"t":3.28404, "x":14.43256, "y":1.06097, "heading":0.05133, "vx":4.31838, "vy":0.41164, "omega":1.26047, "ax":-7.5758, "ay":2.44524, "alpha":23.89803, "fx":[-174.26961,-243.163,-57.64935,-40.36685], "fy":[178.2324,-21.99425,1.98247,8.15045]}, + {"t":3.37787, "x":14.8044, "y":1.11036, "heading":0.2748, "vx":3.60755, "vy":0.64107, "omega":3.50282, "ax":-10.51365, "ay":2.72834, "alpha":20.38802, "fx":[-221.69273,-248.78932,-62.27108,-182.58346], "fy":[114.59431,-18.4419,8.82615,80.65482]}, + {"t":3.4717, "x":15.09662, "y":1.18252, "heading":0.69322, "vx":2.62105, "vy":0.89707, "omega":5.41582, "ax":-11.36371, "ay":4.38917, "alpha":6.23098, "fx":[-225.59535,-249.75934,-222.40629,-75.41256], "fy":[107.48616,1.19641,112.57164,77.38017]}, + {"t":3.56553, "x":15.29253, "y":1.28601, "heading":1.22881, "vx":1.5548, "vy":1.30891, "omega":6.00047, "ax":-12.55633, "ay":2.17907, "alpha":-6.15146, "fx":[-247.93578,-237.9725,-248.85511,-119.55484], "fy":[21.57567,76.11115,19.92715,30.64742]}, + {"t":3.65936, "x":15.38314, "y":1.41842, "heading":1.76476, "vx":0.37664, "vy":1.51337, "omega":5.42328, "ax":-12.12545, "ay":1.76011, "alpha":-11.14981, "fx":[-249.42385,-234.85447,-235.85174,-104.87154], "fy":[14.36944,84.91287,-77.45957,97.93327]}, + {"t":3.72349, "x":15.38236, "y":1.51909, "heading":2.08962, "vx":-0.40094, "vy":1.62624, "omega":4.70826, "ax":-10.08476, "ay":3.33067, "alpha":-0.15548, "fx":[-248.84478,-243.35392,-70.71452,-123.24254], "fy":[21.76117,55.84152,89.88448,59.12763]}, + {"t":3.78762, "x":15.33591, "y":1.63023, "heading":2.39123, "vx":-1.04767, "vy":1.83983, "omega":4.69829, "ax":-10.12325, "ay":1.16098, "alpha":-6.55326, "fx":[-246.75714,-240.2619,-78.0571,-123.69804], "fy":[38.21719,-66.72889,64.95324,42.5501]}, + {"t":3.85175, "x":15.24791, "y":1.7506, "heading":2.67905, "vx":-1.69686, "vy":1.91428, "omega":4.27804, "ax":-6.6578, "ay":3.14715, "alpha":-5.87841, "fx":[-217.85905,-98.12319,-13.63512,-123.37201], "fy":[78.61535,89.00816,10.00053,36.50427]}, + {"t":3.91588, "x":15.1254, "y":1.87983, "heading":2.94131, "vx":-2.12381, "vy":2.11611, "omega":3.90106, "ax":-5.38348, "ay":2.78159, "alpha":-8.75196, "fx":[-174.22421,-85.4983,16.62916,-123.19241], "fy":[84.8562,89.55647,-21.2395,36.0828]}, + {"t":3.98, "x":14.97814, "y":2.02126, "heading":-3.1097, "vx":-2.46905, "vy":2.29449, "omega":3.33981, "ax":-4.6265, "ay":2.37863, "alpha":-9.49293, "fx":[-143.08064,-81.33573,30.57531,-120.94046], "fy":[78.09821,80.03741,-33.13398,36.83791]}, + {"t":4.04413, "x":14.81029, "y":2.17329, "heading":-2.91505, "vx":-2.76574, "vy":2.44702, "omega":2.73105, "ax":-3.90888, "ay":1.68146, "alpha":-9.29256, "fx":[-124.16777,-57.78666,32.24881,-116.25024], "fy":[65.37306,49.7984,-34.6096,33.84287]}, + {"t":4.10826, "x":14.62489, "y":2.33367, "heading":-2.75902, "vx":-3.01641, "vy":2.55485, "omega":2.13513, "ax":-3.09767, "ay":1.0704, "alpha":-8.04096, "fx":[-109.72958,-23.9793,32.61231,-109.66536], "fy":[63.12822,28.37607,-32.98897,14.3132]}, + {"t":4.17239, "x":14.42508, "y":2.49971, "heading":-2.63863, "vx":-3.21506, "vy":2.6235, "omega":1.61947, "ax":-2.43368, "ay":0.85647, "alpha":-6.97988, "fx":[-86.96093,-7.78368,29.17354,-100.01377], "fy":[78.7127,9.60222,-29.29215,-0.74923]}, + {"t":4.23652, "x":14.2139, "y":2.66971, "heading":-2.54912, "vx":-3.37113, "vy":2.67842, "omega":1.17186, "ax":-1.31228, "ay":0.69207, "alpha":-3.90263, "fx":[-42.65786,2.33647,24.65002,-73.6144], "fy":[90.74296,-1.50842,-25.78467,-16.36229]}, + {"t":4.30065, "x":13.99501, "y":2.8429, "heading":-2.482, "vx":-3.45528, "vy":2.7228, "omega":0.92159, "ax":-1.38469, "ay":-1.5441, "alpha":0.56321, "fx":[-90.0206,7.63395,27.71076,-39.537], "fy":[66.13985,-6.21107,-26.65439,-138.33291]}, + {"t":4.36478, "x":13.77058, "y":3.01433, "heading":-2.42174, "vx":-3.54408, "vy":2.62378, "omega":0.95771, "ax":-1.32155, "ay":-1.6448, "alpha":0.08772, "fx":[-93.49181,11.20888,29.03767,-36.67158], "fy":[60.21876,-9.07285,-27.504,-135.55253]}, + {"t":4.4289, "x":13.54059, "y":3.17921, "heading":-2.36014, "vx":-3.62883, "vy":2.5183, "omega":0.96333, "ax":-1.28522, "ay":-1.87071, "alpha":0.11443, "fx":[-97.37902,13.55388,29.51744,-33.1371], "fy":[51.9268,-11.19898,-28.04404,-139.96445]}, + {"t":4.49303, "x":13.30524, "y":3.33686, "heading":-2.29813, "vx":-3.71125, "vy":2.39834, "omega":0.97067, "ax":-1.26745, "ay":-2.09884, "alpha":0.07746, "fx":[-100.87934,14.90525,29.48589,-29.74764], "fy":[41.83894,-12.91127,-28.78545,-142.9451]}, + {"t":4.55716, "x":13.06463, "y":3.48635, "heading":-2.23572, "vx":-3.79253, "vy":2.26374, "omega":0.97564, "ax":-1.2569, "ay":-2.25481, "alpha":-0.2316, "fx":[-103.07555,15.28774,29.02311,-26.75334], "fy":[31.20569,-14.28926,-29.7792,-140.55211]}, + {"t":4.62129, "x":12.81884, "y":3.62688, "heading":-2.17363, "vx":-3.87313, "vy":2.11914, "omega":0.96079, "ax":-1.20651, "ay":-2.27042, "alpha":-0.38807, "fx":[-103.24676,16.07131,27.60652,-22.5203], "fy":[24.26894,-14.31727,-27.72468,-136.70404]}, + {"t":4.69883, "x":12.51488, "y":3.78438, "heading":-2.1003, "vx":-3.96669, "vy":1.94309, "omega":0.9307, "ax":-1.08118, "ay":-1.91741, "alpha":-0.80918, "fx":[-95.62839,17.64848,25.64637,-21.22896], "fy":[23.79131,-12.68762,-22.14835,-119.41392]}, + {"t":4.77637, "x":12.20404, "y":3.92928, "heading":-2.03056, "vx":-4.05052, "vy":1.79441, "omega":0.86795, "ax":-0.91028, "ay":-1.35759, "alpha":-1.436, "fx":[-80.55692,18.52522,23.23193,-23.13462], "fy":[26.34346,-10.62483,-17.3884,-90.69914]}, + {"t":4.85392, "x":11.88722, "y":4.06434, "heading":-1.96758, "vx":-4.12111, "vy":1.68914, "omega":0.7566, "ax":-0.72026, "ay":-0.97277, "alpha":-1.52003, "fx":[-64.62999,18.7377,19.67123,-22.78447], "fy":[25.88653,-9.11537,-13.68235,-69.27525]}, + {"t":4.93146, "x":11.5655, "y":4.1924, "heading":-1.91348, "vx":-4.17696, "vy":1.61371, "omega":0.63873, "ax":-0.51573, "ay":-0.13988, "alpha":-2.30612, "fx":[-33.73471,18.56294,15.88197,-35.79999], "fy":[30.76591,-6.25563,-10.88101,-23.14665]}, + {"t":5.009, "x":11.24006, "y":4.31711, "heading":-1.87089, "vx":-4.21695, "vy":1.60286, "omega":0.45991, "ax":-0.35991, "ay":-0.47262, "alpha":-0.70304, "fx":[-34.34633,15.14175,10.94304,-16.22663], "fy":[20.73609,-5.33884,-7.16704,-40.38652]}, + {"t":5.08654, "x":10.91198, "y":4.43998, "heading":-1.83734, "vx":-4.24486, "vy":1.56621, "omega":0.4054, "ax":-0.26901, "ay":-0.4799, "alpha":-0.38575, "fx":[-30.34421,15.10804,10.03223,-13.09927], "fy":[18.9442,-4.97619,-6.34857,-40.27161]}, + {"t":5.16408, "x":10.58202, "y":4.55998, "heading":-1.80706, "vx":-4.26572, "vy":1.529, "omega":0.37548, "ax":-0.21801, "ay":-0.47329, "alpha":-0.2207, "fx":[-27.77167,15.15011,9.32627,-11.53788], "fy":[17.94398,-4.69536,-5.7604,-39.69007]}, + {"t":5.24163, "x":10.25059, "y":4.67712, "heading":-1.77861, "vx":-4.28262, "vy":1.4923, "omega":0.35837, "ax":-0.18747, "ay":-0.47764, "alpha":-0.1048, "fx":[-26.35844,15.29736,8.74312,-10.43721], "fy":[17.1888,-4.53434,-5.30177,-39.85052]}, + {"t":5.31917, "x":9.91795, "y":4.7914, "heading":-1.75113, "vx":-4.29716, "vy":1.45527, "omega":0.35024, "ax":-0.16861, "ay":-0.49838, "alpha":-0.00578, "fx":[-25.87624,15.61119,8.27244,-9.47932], "fy":[16.55959,-4.50089,-4.9445,-41.02324]}, + {"t":5.39671, "x":9.58423, "y":4.90275, "heading":-1.72399, "vx":-4.31023, "vy":1.41662, "omega":0.3498, "ax":-0.15689, "ay":-0.53873, "alpha":0.09224, "fx":[-26.19631,16.14795,7.90113,-8.5272], "fy":[15.99208,-4.6018,-4.6684,-43.37614]}, + {"t":5.47425, "x":9.24953, "y":5.01098, "heading":-1.69659, "vx":-4.3224, "vy":1.37485, "omega":0.35695, "ax":-0.14917, "ay":-0.60469, "alpha":0.20513, "fx":[-27.29907,16.97801,7.61196,-7.44027], "fy":[15.4116,-4.85841,-4.4549,-47.24098]}, + {"t":5.55179, "x":8.91392, "y":5.11577, "heading":-1.6683, "vx":-4.33397, "vy":1.32796, "omega":0.37286, "ax":-0.14163, "ay":-0.7093, "alpha":0.36068, "fx":[-29.30694,18.23232,7.38764,-5.94966], "fy":[14.71829,-5.32682,-4.28615,-53.36538]}, + {"t":5.62934, "x":8.57743, "y":5.21661, "heading":-1.6383, "vx":-4.34495, "vy":1.27296, "omega":0.40082, "ax":-0.12679, "ay":-0.88042, "alpha":0.61719, "fx":[-32.54188,20.19552,7.20911,-3.48932], "fy":[13.76141,-6.13968,-4.1416,-63.38321]}, + {"t":5.70688, "x":8.24013, "y":5.31267, "heading":-1.60536, "vx":-4.35478, "vy":1.20469, "omega":0.44868, "ax":-0.08583, "ay":-1.18118, "alpha":1.11067, "fx":[-37.68977,23.53125,7.04356,1.27486], "fy":[12.26505,-7.62021,-3.98738,-81.02389]}, + {"t":5.78442, "x":7.90219, "y":5.40253, "heading":-1.56723, "vx":-4.36144, "vy":1.11309, "omega":0.53481, "ax":-0.03607, "ay":-1.64025, "alpha":1.73186, "fx":[-46.18424,28.79663,6.81614,8.11699], "fy":[9.88513,-10.15817,-3.79037,-107.53708]}, + {"t":5.86196, "x":7.56389, "y":5.48391, "heading":-1.52056, "vx":-4.36423, "vy":0.98591, "omega":0.6691, "ax":-0.17782, "ay":-1.9443, "alpha":1.13815, "fx":[-58.17889,33.17647,6.41821,6.48574], "fy":[6.04986,-12.76504,-3.6257,-121.94678]}, + {"t":5.93951, "x":7.22494, "y":5.55451, "heading":-1.46525, "vx":-4.37802, "vy":0.83514, "omega":0.75735, "ax":-0.20944, "ay":-2.21413, "alpha":0.67581, "fx":[-64.19374,36.24786,5.59708,8.09889], "fy":[-0.10204,-15.13878,-3.34582,-132.06014]}, + {"t":6.01705, "x":6.88483, "y":5.61262, "heading":-1.40449, "vx":-4.39426, "vy":0.66345, "omega":0.80976, "ax":-0.10271, "ay":-2.51034, "alpha":0.67143, "fx":[-66.43469,39.20142,3.62159,16.62346], "fy":[-6.9793,-17.8358,-2.37273,-143.6128]}, + {"t":6.09459, "x":6.54378, "y":5.65651, "heading":-1.33968, "vx":-4.40223, "vy":0.4688, "omega":0.86182, "ax":1.64523, "ay":-4.70185, "alpha":5.76429, "fx":[-65.64134,54.96467,41.05737,81.55906], "fy":[-19.08986,-37.43174,-56.7381,-206.6488]}, + {"t":6.17213, "x":6.20737, "y":5.67873, "heading":-1.25553, "vx":-4.27465, "vy":0.10421, "omega":1.30879, "ax":6.90505, "ay":-10.10952, "alpha":3.28375, "fx":[91.50878,76.01835,159.72117,142.56339], "fy":[-228.77797,-70.06989,-184.54257,-204.44967]}, + {"t":6.24967, "x":5.89667, "y":5.65642, "heading":-1.14417, "vx":-3.73922, "vy":-0.67971, "omega":1.56342, "ax":3.76567, "ay":-7.79918, "alpha":6.97339, "fx":[112.30018,25.30289,-58.23373,176.84284], "fy":[-222.18694,-57.47539,-75.55349,-175.43147]}, + {"t":6.31944, "x":5.64496, "y":5.59002, "heading":-1.01813, "vx":-3.47651, "vy":-1.22382, "omega":2.04992, "ax":3.97321, "ay":-6.4108, "alpha":8.50703, "fx":[123.94678,-9.12699,-43.62392,199.13684], "fy":[-215.66451,-8.83864,-68.62764,-143.05257]}, + {"t":6.3892, "x":5.41209, "y":5.48904, "heading":-0.85441, "vx":-3.19931, "vy":-1.67107, "omega":2.64342, "ax":-1.80924, "ay":-3.98604, "alpha":-5.91435, "fx":[-75.05018,27.48257,-34.56242,-40.96828], "fy":[-87.90439,1.4383,-60.43995,-124.29968]}, + {"t":6.45897, "x":5.18449, "y":5.36275, "heading":-0.68439, "vx":-3.32554, "vy":-1.94915, "omega":2.2308, "ax":-1.6224, "ay":-3.35341, "alpha":-6.97295, "fx":[-85.13743,39.14792,-28.64688,-35.74983], "fy":[-75.64736,10.38475,-50.80781,-112.09207]}, + {"t":6.52873, "x":4.94854, "y":5.21861, "heading":-0.54572, "vx":-3.43872, "vy":-2.18311, "omega":1.74433, "ax":-1.29221, "ay":-2.74951, "alpha":-6.51996, "fx":[-85.06971,40.48491,-20.24381,-23.09221], "fy":[-63.15861,15.84778,-37.77266,-101.9898]}, + {"t":6.5985, "x":4.70549, "y":5.05961, "heading":-0.4399, "vx":-3.52887, "vy":-2.37493, "omega":1.28947, "ax":2.04175, "ay":-2.63888, "alpha":4.26158, "fx":[-86.85284,62.58331,-21.38295,184.57101], "fy":[-58.82332,33.39351,-36.42687,-117.68948]}, + {"t":6.66826, "x":4.46426, "y":4.88751, "heading":-0.33956, "vx":-3.38643, "vy":-2.55903, "omega":1.58678, "ax":0.34226, "ay":-1.89119, "alpha":-2.5291, "fx":[-70.14696,59.20569,-18.35344,52.58158], "fy":[-35.14703,37.55331,-37.24657,-93.8339]}, + {"t":6.73803, "x":4.22884, "y":4.70437, "heading":-0.23502, "vx":-3.36255, "vy":-2.69097, "omega":1.41034, "ax":1.05655, "ay":-1.73318, "alpha":-0.76537, "fx":[-57.91529,56.75258,-18.28739,91.33656], "fy":[-28.45949,41.25091,-38.07958,-92.63538]}, + {"t":6.80779, "x":3.99682, "y":4.51242, "heading":-0.13849, "vx":-3.28884, "vy":-2.81188, "omega":1.35694, "ax":1.43399, "ay":-1.65562, "alpha":-0.04879, "fx":[-50.63539,54.59955,-18.11527,111.71833], "fy":[-23.30669,44.32924,-42.8093,-90.85982]}, + {"t":6.87756, "x":3.77087, "y":4.31222, "heading":-0.04394, "vx":-3.1888, "vy":-2.92739, "omega":1.35354, "ax":1.71337, "ay":-1.63856, "alpha":0.29951, "fx":[-45.05047,52.52007,-17.57681,126.68288], "fy":[-20.31414,46.73836,-50.24293,-87.66728]}, + {"t":6.94732, "x":3.55257, "y":4.104, "heading":0.05122, "vx":-3.06927, "vy":-3.0417, "omega":1.37443, "ax":2.55499, "ay":-1.54263, "alpha":1.80847, "fx":[-45.02989,49.78823,191.74863,-22.66837], "fy":[-19.6304,49.93354,-36.013,-99.24882]}, + {"t":7.01709, "x":3.34466, "y":3.88804, "heading":0.15151, "vx":-2.89102, "vy":-3.14932, "omega":1.5006, "ax":5.65352, "ay":-0.96428, "alpha":10.04074, "fx":[-45.4275,55.05081,141.4602,233.57565], "fy":[-23.07785,68.05229,-38.4464,-72.13623]}, + {"t":7.08685, "x":3.15673, "y":3.66598, "heading":0.28063, "vx":-2.4966, "vy":-3.21659, "omega":2.20109, "ax":6.98038, "ay":2.77806, "alpha":20.5179, "fx":[-64.02752,63.86514,242.72792,232.37153], "fy":[-31.65241,91.85075,40.82585,87.99154]}, + {"t":7.15662, "x":2.99954, "y":3.44834, "heading":0.48412, "vx":-2.00961, "vy":-3.02278, "omega":3.63253, "ax":7.62319, "ay":4.35173, "alpha":18.13268, "fx":[-46.04818,122.17257,225.07174,217.47722], "fy":[-25.52065,93.5042,106.10464,121.99867]}, + {"t":7.22639, "x":2.87789, "y":3.24804, "heading":0.78167, "vx":-1.47778, "vy":-2.71918, "omega":4.89755, "ax":12.55912, "ay":6.02162, "alpha":-3.78139, "fx":[247.54592,214.70065,182.4243,209.83698], "fy":[-21.92334,126.82097,169.553,135.25376]}, + {"t":7.29615, "x":2.80536, "y":3.07299, "heading":1.11415, "vx":-0.60159, "vy":-2.29909, "omega":4.63375, "ax":11.88032, "ay":6.18205, "alpha":-7.29916, "fx":[242.36845,190.50784,161.70808,213.73917], "fy":[59.99695,161.24277,70.10927,129.27039]}, + {"t":7.36592, "x":2.7923, "y":2.92764, "heading":1.41966, "vx":0.22724, "vy":-1.86779, "omega":4.12452, "ax":11.7231, "ay":5.58661, "alpha":-16.49331, "fx":[234.01143,131.08971,196.52403,236.0009], "fy":[87.54892,212.46302,-1.97578,82.07071]}, + {"t":7.43568, "x":2.83668, "y":2.81093, "heading":1.66727, "vx":1.0451, "vy":-1.47804, "omega":2.97386, "ax":7.75685, "ay":7.78551, "alpha":-23.00972, "fx":[86.69423,47.65743,190.70423,202.71118], "fy":[233.77073,241.52887,26.2424,28.17519]}, + {"t":7.53638, "x":2.98125, "y":2.70157, "heading":1.85007, "vx":1.82621, "vy":-0.69405, "omega":0.65682, "ax":8.77614, "ay":1.72279, "alpha":-3.9474, "fx":[144.87264,131.88799,157.28308,163.07462], "fy":[38.73714,50.29192,15.1762,13.01162]}, + {"t":7.63708, "x":3.20964, "y":2.64041, "heading":1.8962, "vx":2.70995, "vy":-0.52057, "omega":0.25932, "ax":7.46574, "ay":1.2515, "alpha":-2.23112, "fx":[123.92365,120.56679,131.58519,131.885], "fy":[29.4065,34.00008,10.28968,11.45423]}, + {"t":7.73778, "x":3.52038, "y":2.59434, "heading":1.911, "vx":3.46174, "vy":-0.39455, "omega":0.03465, "ax":6.29891, "ay":1.41199, "alpha":-1.23063, "fx":[104.66335,104.11216,110.49104,109.3042], "fy":[29.05464,31.91577,17.67545,17.42424]}, + {"t":7.83847, "x":3.90091, "y":2.56176, "heading":1.90825, "vx":4.09603, "vy":-0.25236, "omega":-0.08927, "ax":2.67412, "ay":1.20527, "alpha":-0.32487, "fx":[40.83904,44.76879,54.89015,41.44625], "fy":[20.49494,34.51634,16.48226,10.51156]}, + {"t":7.93917, "x":4.32693, "y":2.54246, "heading":1.89761, "vx":4.36531, "vy":-0.131, "omega":-0.12198, "ax":1.23508, "ay":0.46489, "alpha":0.35304, "fx":[17.53236,24.33776,26.3267,15.8365], "fy":[7.54619,14.53007,6.19824,3.35605]}, + {"t":8.03987, "x":4.77277, "y":2.53163, "heading":1.88712, "vx":4.48968, "vy":-0.08418, "omega":-0.08643, "ax":0.54659, "ay":0.18158, "alpha":0.3467, "fx":[7.22128,12.11305,12.12625,5.7286], "fy":[2.77114,6.14482,2.38126,1.05728]}, + {"t":8.14057, "x":5.22764, "y":2.52407, "heading":1.88017, "vx":4.54472, "vy":-0.0659, "omega":-0.05152, "ax":0.23623, "ay":0.07026, "alpha":0.22904, "fx":[2.89785,5.7989,5.44684,1.92901], "fy":[1.00462,2.56758,0.89655,0.3117]}, + {"t":8.24127, "x":5.68649, "y":2.51779, "heading":1.87615, "vx":4.56851, "vy":-0.05882, "omega":-0.02846, "ax":0.09942, "ay":0.02602, "alpha":0.13054, "fx":[1.13167,2.69897,2.38984,0.54403], "fy":[0.35286,1.05597,0.2905,0.07109]}, + {"t":8.34197, "x":6.14703, "y":2.512, "heading":1.87394, "vx":4.57852, "vy":-0.0562, "omega":-0.01531, "ax":0.03966, "ay":0.00878, "alpha":0.06919, "fx":[0.42591,1.21215,1.00418,0.05592], "fy":[0.11726,0.41557,0.05913,0.00509]}, + {"t":8.44266, "x":6.60828, "y":2.50639, "heading":1.87275, "vx":4.58251, "vy":-0.05532, "omega":-0.00835, "ax":0.01423, "ay":0.00274, "alpha":0.03468, "fx":[0.15269,0.50285,0.38833,-0.07584], "fy":[0.03632,0.14349,0.00103,0.00579]}, + {"t":8.54336, "x":7.06981, "y":2.50083, "heading":1.87209, "vx":4.58394, "vy":-0.05504, "omega":-0.00485, "ax":0.0044, "ay":0.00074, "alpha":0.01466, "fx":[0.05708,0.17706,0.13458,-0.06938], "fy":[0.01178,0.04013,-0.00463,0.00334]}, + {"t":8.64406, "x":7.53142, "y":2.49529, "heading":1.87167, "vx":4.58439, "vy":-0.05497, "omega":-0.00338, "ax":0.00073, "ay":0.00014, "alpha":0.00432, "fx":[0.02733,0.02897,0.04163,-0.04849], "fy":[0.0053,0.00587,-0.00306,0.00107]}, + {"t":8.74476, "x":7.99307, "y":2.48976, "heading":1.87135, "vx":4.58446, "vy":-0.05495, "omega":-0.00294, "ax":-0.00318, "ay":0.00046, "alpha":-0.00733, "fx":[-0.0004,-0.15893,-0.00067,-0.05629], "fy":[0.00366,0.03082,-0.00288,-0.0005]}, + {"t":8.84546, "x":8.4547, "y":2.48423, "heading":1.87102, "vx":4.58414, "vy":-0.05491, "omega":-0.00368, "ax":-1.62162, "ay":1.54925, "alpha":-7.11959, "fx":[-1.51174,-85.40677,-2.07881,-21.33569], "fy":[0.37856,106.69438,-0.71566,-0.94824]}, + {"t":8.94616, "x":8.90809, "y":2.48655, "heading":1.83455, "vx":4.42084, "vy":0.1011, "omega":-0.72061, "ax":-14.66328, "ay":-0.24243, "alpha":-1.66151, "fx":[-249.50469,-249.62041,-249.19127,-249.35655], "fy":[6.91477,2.09653,-14.66682,-10.83899]}, + {"t":9.04685, "x":9.27892, "y":2.4955, "heading":1.75357, "vx":2.94428, "vy":0.07669, "omega":-0.88792, "ax":-14.63916, "ay":-0.36492, "alpha":3.68282, "fx":[-248.39121,-248.68827,-249.22442,-249.72768], "fy":[-27.19552,-24.03119,17.70149,8.69682]}, + {"t":9.14755, "x":9.50118, "y":2.50138, "heading":1.68283, "vx":1.47014, "vy":0.03994, "omega":-0.51707, "ax":-14.59941, "ay":-0.39664, "alpha":5.13483, "fx":[-247.72024,-247.51855,-248.61275,-249.47545], "fy":[-33.24459,-34.53478,25.51885,15.27329]}, + {"t":9.24825, "x":9.5752, "y":2.50339, "heading":1.65679, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.02218, "ay":-0.00101, "alpha":-0.00002, "fx":[0.37718,0.37732,0.37728,0.37715], "fy":[-0.01703,-0.01708,-0.0174,-0.01735]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-medium-dt/project.chor b/test-projects/swerve-weak-medium-dt/project.chor new file mode 100755 index 0000000000..14e63be164 --- /dev/null +++ b/test-projects/swerve-weak-medium-dt/project.chor @@ -0,0 +1,110 @@ +{ + "name":"project", + "version":3, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"125 lbs", + "val":56.69904625 + }, + "inertia":{ + "exp":"5 kg m ^ 2", + "val":5.0 + }, + "gearing":{ + "exp":"6.03", + "val":6.03 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + }, + "motorConfig":{ + "free_speed":{ + "exp":"5906 RPM", + "val":618.4748737367106 + }, + "stall_torque":{ + "exp":"4.2 N * m", + "val":4.2 + }, + "kT":{ + "exp":"0.0194 N * m / A", + "val":0.0194 + }, + "kV":{ + "exp":"0.002014 V / RPM", + "val":0.019232283323224637 + }, + "supply_limit":{ + "exp":"40 A", + "val":40.0 + }, + "stator_limit":{ + "exp":"40 A", + "val":40.0 + } + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/test-projects/swerve-weak-medium-dt/small_curve.traj b/test-projects/swerve-weak-medium-dt/small_curve.traj new file mode 100644 index 0000000000..b80bf50d68 --- /dev/null +++ b/test-projects/swerve-weak-medium-dt/small_curve.traj @@ -0,0 +1,108 @@ +{ + "name":"small_curve", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.6358277797698977, "y":5.847504615783691, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.0072877407073975, "y":3.0705108642578125, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.673925399780273, "y":5.577029705047607, "heading":0.0, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.6358277797698975 m", "val":1.6358277797698977}, "y":{"exp":"5.847504615783691 m", "val":5.847504615783691}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.0072877407073975 m", "val":3.0072877407073975}, "y":{"exp":"3.0705108642578125 m", "val":3.0705108642578125}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.673925399780273 m", "val":5.673925399780273}, "y":{"exp":"5.577029705047607 m", "val":5.577029705047607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":272, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.96196,2.08546], + "samples":[ + {"t":0.0, "x":1.63583, "y":5.8475, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.05713, "ay":-14.11526, "alpha":0.0, "fx":[69.01069,69.01069,69.0107,69.0107], "fy":[-240.09648,-240.09648,-240.09648,-240.09648]}, + {"t":0.05063, "x":1.64103, "y":5.82941, "heading":0.0, "vx":0.20541, "vy":-0.71465, "omega":0.0, "ax":4.08231, "ay":-13.62972, "alpha":0.0, "fx":[69.43889,69.43889,69.43889,69.43889], "fy":[-231.8376,-231.83759,-231.83759,-231.83759]}, + {"t":0.10126, "x":1.65666, "y":5.77576, "heading":0.0, "vx":0.41209, "vy":-1.40471, "omega":0.0, "ax":3.50786, "ay":-11.29489, "alpha":0.0, "fx":[59.6677,59.6677,59.6677,59.6677], "fy":[-192.12285,-192.12285,-192.12285,-192.12285]}, + {"t":0.15189, "x":1.68202, "y":5.69017, "heading":0.0, "vx":0.5897, "vy":-1.97656, "omega":0.0, "ax":3.12326, "ay":-9.66636, "alpha":0.0, "fx":[53.12578,53.12578,53.12578,53.12578], "fy":[-164.42202,-164.42202,-164.42202,-164.42202]}, + {"t":0.20252, "x":1.71588, "y":5.57771, "heading":0.0, "vx":0.74782, "vy":-2.46596, "omega":0.0, "ax":2.86749, "ay":-8.46051, "alpha":0.0, "fx":[48.77518,48.77518,48.77518,48.77518], "fy":[-143.91078,-143.91078,-143.91078,-143.91078]}, + {"t":0.25315, "x":1.75742, "y":5.44201, "heading":0.0, "vx":0.893, "vy":-2.89431, "omega":0.0, "ax":2.71387, "ay":-7.51777, "alpha":0.0, "fx":[46.16212,46.16212,46.16212,46.16212], "fy":[-127.87516,-127.87516,-127.87516,-127.87516]}, + {"t":0.30378, "x":1.80611, "y":5.28584, "heading":0.0, "vx":1.0304, "vy":-3.27493, "omega":0.0, "ax":2.66534, "ay":-6.73264, "alpha":0.0, "fx":[45.33672,45.33672,45.33672,45.33672], "fy":[-114.5202,-114.5202,-114.5202,-114.5202]}, + {"t":0.35441, "x":1.86169, "y":5.1114, "heading":0.0, "vx":1.16535, "vy":-3.6158, "omega":0.0, "ax":2.76942, "ay":-5.985, "alpha":0.0, "fx":[47.10701,47.10701,47.10701,47.10701], "fy":[-101.80317,-101.80317,-101.80317,-101.80317]}, + {"t":0.40503, "x":1.92424, "y":4.92067, "heading":0.0, "vx":1.30556, "vy":-3.91882, "omega":0.0, "ax":1.92495, "ay":-3.32146, "alpha":0.0, "fx":[32.74287,32.74287,32.74287,32.74287], "fy":[-56.49706,-56.49706,-56.49706,-56.49706]}, + {"t":0.45566, "x":1.99281, "y":4.718, "heading":0.0, "vx":1.40302, "vy":-4.08698, "omega":0.0, "ax":1.22617, "ay":-1.85609, "alpha":0.0, "fx":[20.85687,20.85687,20.85687,20.85687], "fy":[-31.57157,-31.57157,-31.57157,-31.57157]}, + {"t":0.50629, "x":2.06541, "y":4.5087, "heading":0.0, "vx":1.4651, "vy":-4.18096, "omega":0.0, "ax":0.78667, "ay":-1.03457, "alpha":0.0, "fx":[13.38097,13.38097,13.38097,13.38097], "fy":[-17.59766,-17.59766,-17.59766,-17.59766]}, + {"t":0.55692, "x":2.1406, "y":4.2957, "heading":0.0, "vx":1.50493, "vy":-4.23334, "omega":0.0, "ax":0.50909, "ay":-0.5756, "alpha":0.0, "fx":[8.65951,8.65951,8.65951,8.65951], "fy":[-9.79076,-9.79076,-9.79076,-9.79076]}, + {"t":0.60755, "x":2.21745, "y":4.08063, "heading":0.0, "vx":1.53071, "vy":-4.26248, "omega":0.0, "ax":0.33222, "ay":-0.32069, "alpha":0.0, "fx":[5.65097,5.65097,5.65097,5.65097], "fy":[-5.45484,-5.45484,-5.45484,-5.45484]}, + {"t":0.65818, "x":2.29537, "y":3.86441, "heading":0.0, "vx":1.54753, "vy":-4.27871, "omega":0.0, "ax":4.09739, "ay":6.02444, "alpha":0.0, "fx":[69.6953,69.69527,69.69599,69.69543], "fy":[102.47376,102.47371,102.47488,102.47397]}, + {"t":0.70881, "x":2.37897, "y":3.6555, "heading":0.0, "vx":1.75497, "vy":-3.9737, "omega":0.0, "ax":8.24757, "ay":11.96408, "alpha":0.0, "fx":[140.28874,140.28874,140.28874,140.28874], "fy":[203.50551,203.50551,203.50551,203.50551]}, + {"t":0.75944, "x":2.4784, "y":3.46965, "heading":0.0, "vx":2.17254, "vy":-3.36797, "omega":0.0, "ax":6.41544, "ay":13.15948, "alpha":0.0, "fx":[109.12479,109.12479,109.12479,109.12479], "fy":[223.83903,223.83903,223.83903,223.83903]}, + {"t":0.81007, "x":2.59661, "y":3.316, "heading":0.0, "vx":2.49735, "vy":-2.70171, "omega":0.0, "ax":4.19375, "ay":14.0523, "alpha":0.0, "fx":[71.33456,71.33456,71.33456,71.33456], "fy":[239.02563,239.02563,239.02563,239.02563]}, + {"t":0.8607, "x":2.72843, "y":3.19722, "heading":0.0, "vx":2.70968, "vy":-1.99025, "omega":0.0, "ax":1.61012, "ay":14.58573, "alpha":0.0, "fx":[27.38767,27.38767,27.38767,27.38767], "fy":[248.09916,248.09916,248.09916,248.09916]}, + {"t":0.91133, "x":2.86768, "y":3.11515, "heading":0.0, "vx":2.7912, "vy":-1.25179, "omega":0.0, "ax":-1.33281, "ay":14.61771, "alpha":0.0, "fx":[-22.67073,-22.67073,-22.67073,-22.67073], "fy":[248.64302,248.64302,248.64302,248.64302]}, + {"t":0.96196, "x":3.00729, "y":3.07051, "heading":0.0, "vx":2.72372, "vy":-0.5117, "omega":0.0, "ax":-4.75002, "ay":13.88433, "alpha":0.0, "fx":[-80.79643,-80.79643,-80.79643,-80.79643], "fy":[236.16844,236.16844,236.16844,236.16844]}, + {"t":1.01546, "x":3.14621, "y":3.06301, "heading":0.0, "vx":2.46959, "vy":0.23111, "omega":0.0, "ax":-8.28152, "ay":12.08213, "alpha":0.0, "fx":[-140.86624,-140.86624,-140.86624,-140.86624], "fy":[205.51353,205.51353,205.51353,205.51353]}, + {"t":1.06896, "x":3.26648, "y":3.09266, "heading":0.0, "vx":2.02653, "vy":0.87751, "omega":0.0, "ax":3.38591, "ay":8.11649, "alpha":0.0, "fx":[57.59334,57.59334,57.59334,57.59334], "fy":[138.05921,138.05921,138.05921,138.05921]}, + {"t":1.12246, "x":3.37975, "y":3.15122, "heading":0.0, "vx":2.20768, "vy":1.31174, "omega":0.0, "ax":3.65522, "ay":7.41688, "alpha":0.0, "fx":[62.1742,62.1742,62.17421,62.17421], "fy":[126.15899,126.15899,126.15899,126.15899]}, + {"t":1.17596, "x":3.50309, "y":3.23202, "heading":0.0, "vx":2.40323, "vy":1.70855, "omega":0.0, "ax":3.44039, "ay":6.81606, "alpha":0.0, "fx":[58.52005,58.52005,58.52005,58.52005], "fy":[115.9393,115.9393,115.9393,115.9393]}, + {"t":1.22946, "x":3.63659, "y":3.33318, "heading":0.0, "vx":2.58729, "vy":2.07321, "omega":0.0, "ax":3.01205, "ay":6.33254, "alpha":0.0, "fx":[51.23405,51.23405,51.23405,51.23405], "fy":[107.71465,107.71465,107.71465,107.71465]}, + {"t":1.28296, "x":3.77932, "y":3.45316, "heading":0.0, "vx":2.74844, "vy":2.412, "omega":0.0, "ax":2.35543, "ay":5.95941, "alpha":0.0, "fx":[40.06521,40.06521,40.06521,40.06521], "fy":[101.36779,101.36779,101.36779,101.36779]}, + {"t":1.33646, "x":3.92973, "y":3.59073, "heading":0.0, "vx":2.87445, "vy":2.73083, "omega":0.0, "ax":0.97164, "ay":5.04183, "alpha":0.0, "fx":[16.5274,16.5274,16.5274,16.5274], "fy":[85.76005,85.76005,85.76005,85.76005]}, + {"t":1.38996, "x":4.0849, "y":3.74405, "heading":0.0, "vx":2.92644, "vy":3.00057, "omega":0.0, "ax":0.88472, "ay":3.12158, "alpha":0.0, "fx":[15.04889,15.04889,15.04889,15.04889], "fy":[53.09718,53.09718,53.09718,53.09718]}, + {"t":1.44346, "x":4.24274, "y":3.90904, "heading":0.0, "vx":2.97377, "vy":3.16757, "omega":0.0, "ax":0.67762, "ay":1.85862, "alpha":0.0, "fx":[11.52604,11.52604,11.52604,11.52604], "fy":[31.61456,31.61456,31.61456,31.61456]}, + {"t":1.49696, "x":4.4028, "y":4.08117, "heading":0.0, "vx":3.01002, "vy":3.26701, "omega":0.0, "ax":0.46744, "ay":1.07748, "alpha":0.0, "fx":[7.95109,7.95109,7.95109,7.95109], "fy":[18.32763,18.32763,18.32763,18.32763]}, + {"t":1.55046, "x":4.56451, "y":4.2575, "heading":0.0, "vx":3.03503, "vy":3.32466, "omega":0.0, "ax":0.30399, "ay":0.61154, "alpha":0.0, "fx":[5.17085,5.17085,5.17085,5.17085], "fy":[10.40216,10.40216,10.40216,10.40216]}, + {"t":1.60396, "x":4.72732, "y":4.43624, "heading":0.0, "vx":3.05129, "vy":3.35737, "omega":0.0, "ax":0.18882, "ay":0.3388, "alpha":0.0, "fx":[3.21173,3.21173,3.21173,3.21173], "fy":[5.76289,5.76289,5.76289,5.76289]}, + {"t":1.65746, "x":4.89083, "y":4.61635, "heading":0.0, "vx":3.0614, "vy":3.3755, "omega":0.0, "ax":0.10896, "ay":0.17757, "alpha":0.0, "fx":[1.8533,1.8533,1.8533,1.8533], "fy":[3.02046,3.02046,3.02046,3.02046]}, + {"t":1.71096, "x":5.05477, "y":4.79719, "heading":0.0, "vx":3.06723, "vy":3.385, "omega":0.0, "ax":0.03896, "ay":0.0602, "alpha":0.0, "fx":[0.66277,0.66277,0.66277,0.66277], "fy":[1.02392,1.02392,1.02392,1.02392]}, + {"t":1.76446, "x":5.21893, "y":4.97837, "heading":0.0, "vx":3.06931, "vy":3.38822, "omega":0.0, "ax":-13.39068, "ay":-4.63488, "alpha":0.0, "fx":[-227.77156,-227.77156,-227.77159,-227.77159], "fy":[-78.83805,-78.83806,-78.83796,-78.83795]}, + {"t":1.81796, "x":5.36397, "y":5.15301, "heading":0.0, "vx":2.35291, "vy":3.14025, "omega":0.0, "ax":-9.27364, "ay":-11.33049, "alpha":0.0, "fx":[-157.74198,-157.74198,-157.74197,-157.74197], "fy":[-192.72839,-192.7284,-192.72841,-192.7284]}, + {"t":1.87146, "x":5.47658, "y":5.3048, "heading":0.0, "vx":1.85677, "vy":2.53407, "omega":0.0, "ax":-8.84463, "ay":-11.7038, "alpha":0.0, "fx":[-150.44467,-150.44467,-150.44467,-150.44467], "fy":[-199.07826,-199.07826,-199.07826,-199.07826]}, + {"t":1.92496, "x":5.56326, "y":5.42362, "heading":0.0, "vx":1.38358, "vy":1.90791, "omega":0.0, "ax":-8.68973, "ay":-11.83146, "alpha":0.0, "fx":[-147.80976,-147.80976,-147.80976,-147.80976], "fy":[-201.24983,-201.24983,-201.24983,-201.24983]}, + {"t":1.97846, "x":5.62485, "y":5.50877, "heading":0.0, "vx":0.91867, "vy":1.27493, "omega":0.0, "ax":-8.61001, "ay":-11.89577, "alpha":0.0, "fx":[-146.45383,-146.45383,-146.45383,-146.45383], "fy":[-202.34363,-202.34363,-202.34363,-202.34363]}, + {"t":2.03196, "x":5.66167, "y":5.55995, "heading":0.0, "vx":0.45804, "vy":0.6385, "omega":0.0, "ax":-8.56141, "ay":-11.93458, "alpha":0.0, "fx":[-145.62711,-145.62711,-145.62711,-145.62711], "fy":[-203.00385,-203.00385,-203.00385,-203.00385]}, + {"t":2.08546, "x":5.67393, "y":5.57703, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00012, "ay":0.00011, "alpha":0.0, "fx":[0.00201,0.00201,0.00201,0.00201], "fy":[0.00189,0.00189,0.00189,0.00189]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-medium-dt/straight_line.traj b/test-projects/swerve-weak-medium-dt/straight_line.traj new file mode 100644 index 0000000000..47955d6da4 --- /dev/null +++ b/test-projects/swerve-weak-medium-dt/straight_line.traj @@ -0,0 +1,102 @@ +{ + "name":"straight_line", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.3901887238025666, "y":0.8238424062728882, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":16.214420318603516, "y":0.7539268136024475, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.39018872380256653 m", "val":0.3901887238025666}, "y":{"exp":"0.8238424062728882 m", "val":0.8238424062728882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"16.214420318603516 m", "val":16.214420318603516}, "y":{"exp":"0.7539268136024475 m", "val":0.7539268136024475}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.82304], + "samples":[ + {"t":0.0, "x":0.39019, "y":0.82384, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":14.09192, "ay":-0.06226, "alpha":0.0, "fx":[239.6996,239.6996,239.69961,239.69961], "fy":[-1.05906,-1.05905,-1.05905,-1.05906]}, + {"t":0.1062, "x":0.46965, "y":0.82349, "heading":0.0, "vx":1.4965, "vy":-0.00661, "omega":0.0, "ax":10.17037, "ay":-0.04494, "alpha":0.0, "fx":[172.99506,172.99506,172.99506,172.99506], "fy":[-0.76434,-0.76434,-0.76434,-0.76434]}, + {"t":0.21239, "x":0.68592, "y":0.82254, "heading":0.0, "vx":2.57654, "vy":-0.01138, "omega":0.0, "ax":8.02261, "ay":-0.03545, "alpha":0.0, "fx":[136.46231,136.46231,136.46231,136.46231], "fy":[-0.60293,-0.60293,-0.60293,-0.60293]}, + {"t":0.31859, "x":1.00477, "y":0.82113, "heading":0.0, "vx":3.42851, "vy":-0.01515, "omega":0.0, "ax":6.66135, "ay":-0.02943, "alpha":0.0, "fx":[113.30764,113.30764,113.30764,113.30764], "fy":[-0.50062,-0.50062,-0.50062,-0.50062]}, + {"t":0.42478, "x":1.40643, "y":0.81935, "heading":0.0, "vx":4.13591, "vy":-0.01827, "omega":0.0, "ax":2.59737, "ay":-0.01148, "alpha":0.0, "fx":[44.1805,44.1805,44.1805,44.1805], "fy":[-0.1952,-0.1952,-0.1952,-0.1952]}, + {"t":0.53098, "x":1.86029, "y":0.81735, "heading":0.0, "vx":4.41174, "vy":-0.01949, "omega":0.0, "ax":1.00831, "ay":-0.00445, "alpha":0.0, "fx":[17.15108,17.15108,17.15108,17.15108], "fy":[-0.07578,-0.07578,-0.07578,-0.07578]}, + {"t":0.63717, "x":2.33448, "y":0.81525, "heading":0.0, "vx":4.51882, "vy":-0.01997, "omega":0.0, "ax":0.39091, "ay":-0.00173, "alpha":0.0, "fx":[6.64923,6.64923,6.64923,6.64923], "fy":[-0.02938,-0.02938,-0.02938,-0.02938]}, + {"t":0.74337, "x":2.81656, "y":0.81312, "heading":0.0, "vx":4.56033, "vy":-0.02015, "omega":0.0, "ax":0.15058, "ay":-0.00067, "alpha":0.0, "fx":[2.56137,2.56137,2.56137,2.56137], "fy":[-0.01132,-0.01132,-0.01132,-0.01132]}, + {"t":0.84956, "x":3.3017, "y":0.81098, "heading":0.0, "vx":4.57633, "vy":-0.02022, "omega":0.0, "ax":0.05649, "ay":-0.00025, "alpha":0.0, "fx":[0.96087,0.96087,0.96087,0.96087], "fy":[-0.00425,-0.00425,-0.00425,-0.00425]}, + {"t":0.95576, "x":3.788, "y":0.80883, "heading":0.0, "vx":4.58232, "vy":-0.02025, "omega":0.0, "ax":0.01952, "ay":-0.00009, "alpha":0.0, "fx":[0.33204,0.33204,0.33204,0.33204], "fy":[-0.00147,-0.00147,-0.00147,-0.00147]}, + {"t":1.06195, "x":4.27473, "y":0.80668, "heading":0.0, "vx":4.5844, "vy":-0.02026, "omega":0.0, "ax":0.00578, "ay":-0.00003, "alpha":0.0, "fx":[0.09837,0.09837,0.09837,0.09837], "fy":[-0.00043,-0.00043,-0.00043,-0.00043]}, + {"t":1.16815, "x":4.76161, "y":0.80453, "heading":0.0, "vx":4.58501, "vy":-0.02026, "omega":0.0, "ax":0.00149, "ay":-0.00001, "alpha":0.0, "fx":[0.02533,0.02533,0.02533,0.02533], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":1.27435, "x":5.24852, "y":0.80238, "heading":0.0, "vx":4.58517, "vy":-0.02026, "omega":0.0, "ax":0.00036, "ay":0.0, "alpha":0.0, "fx":[0.00612,0.00612,0.00612,0.00612], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.38054, "x":5.73545, "y":0.80023, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":0.00009, "ay":0.0, "alpha":0.0, "fx":[0.00145,0.00145,0.00145,0.00145], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.48674, "x":6.22238, "y":0.79807, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.00002, "ay":0.0, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59293, "x":6.70931, "y":0.79592, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69913, "x":7.19624, "y":0.79377, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.80532, "x":7.68317, "y":0.79162, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91152, "x":8.1701, "y":0.78947, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01771, "x":8.65703, "y":0.78732, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.12391, "x":9.14396, "y":0.78517, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.2301, "x":9.63089, "y":0.78301, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.3363, "x":10.11781, "y":0.78086, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.4425, "x":10.60474, "y":0.77871, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.54869, "x":11.09167, "y":0.77656, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.65489, "x":11.5786, "y":0.77441, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.76108, "x":12.06553, "y":0.77226, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.86728, "x":12.55246, "y":0.77011, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.97347, "x":13.03939, "y":0.76795, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.07967, "x":13.52632, "y":0.7658, "heading":0.0, "vx":4.58522, "vy":-0.02026, "omega":0.0, "ax":-0.00003, "ay":0.0, "alpha":0.0, "fx":[-0.00058,-0.00058,-0.00058,-0.00058], "fy":[0.0,0.0,0.0,0.0]}, + {"t":3.18586, "x":14.01325, "y":0.76365, "heading":0.0, "vx":4.58521, "vy":-0.02026, "omega":0.0, "ax":-0.00015, "ay":0.0, "alpha":0.0, "fx":[-0.0025,-0.0025,-0.0025,-0.0025], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":3.29206, "x":14.50018, "y":0.7615, "heading":0.0, "vx":4.5852, "vy":-0.02026, "omega":0.0, "ax":-0.00065, "ay":0.0, "alpha":0.0, "fx":[-0.01107,-0.01107,-0.01107,-0.01107], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.39825, "x":14.9871, "y":0.75935, "heading":0.0, "vx":4.58513, "vy":-0.02026, "omega":0.0, "ax":-0.00334, "ay":0.00001, "alpha":0.0, "fx":[-0.05688,-0.05688,-0.05688,-0.05688], "fy":[0.00025,0.00025,0.00025,0.00025]}, + {"t":3.50445, "x":15.474, "y":0.7572, "heading":0.0, "vx":4.58477, "vy":-0.02026, "omega":0.0, "ax":-13.79683, "ay":0.06096, "alpha":0.0, "fx":[-234.68005,-234.68004,-234.68012,-234.68011], "fy":[1.03695,1.04003,1.03369,1.03693]}, + {"t":3.61065, "x":15.88309, "y":0.75539, "heading":0.0, "vx":3.11961, "vy":-0.01378, "omega":0.0, "ax":-14.68418, "ay":0.06488, "alpha":0.0, "fx":[-249.77366,-249.77366,-249.77366,-249.77366], "fy":[1.10355,1.10355,1.10355,1.10355]}, + {"t":3.71684, "x":16.13158, "y":0.75429, "heading":0.0, "vx":1.56022, "vy":-0.00689, "omega":0.0, "ax":-14.69198, "ay":0.06491, "alpha":0.0, "fx":[-249.90645,-249.90645,-249.90645,-249.90645], "fy":[1.10415,1.10415,1.10415,1.10415]}, + {"t":3.82304, "x":16.21442, "y":0.75393, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.00011, "ay":0.0, "alpha":0.0, "fx":[0.00188,0.00188,0.00188,0.00188], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}], + "splits":[0] + }, + "events":[] +} diff --git a/test-projects/swerve-weak-medium-dt/sweep_auto.traj b/test-projects/swerve-weak-medium-dt/sweep_auto.traj new file mode 100644 index 0000000000..59bb291fed --- /dev/null +++ b/test-projects/swerve-weak-medium-dt/sweep_auto.traj @@ -0,0 +1,157 @@ +{ + "name":"sweep_auto", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.428445816040039, "y":0.5943955779075623, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.65521240234375, "y":0.5362290740013123, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.693989753723145, "y":6.314103126525879, "heading":1.540953960441231, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.522439956665039, "y":6.643713474273682, "heading":-2.1061292676810126, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.428445816040039 m", "val":4.428445816040039}, "y":{"exp":"0.5943955779075623 m", "val":0.5943955779075623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.65521240234375 m", "val":8.65521240234375}, "y":{"exp":"0.5362290740013123 m", "val":0.5362290740013123}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.693989753723145 m", "val":8.693989753723145}, "y":{"exp":"6.314103126525879 m", "val":6.314103126525879}, "heading":{"exp":"1.540953960441231 rad", "val":1.540953960441231}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.522439956665039 m", "val":6.522439956665039}, "y":{"exp":"6.643713474273682 m", "val":6.643713474273682}, "heading":{"exp":"-2.1061292676810126 rad", "val":-2.1061292676810126}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588, + "motorConfig":{ + "free_speed":607.3745796940267, + "stall_torque":9.36, + "kT":0.02, + "kV":0.01975716537568162, + "supply_limit":60.0, + "stator_limit":120.0 + } + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.21564,2.66087,3.46297], + "samples":[ + {"t":0.0, "x":4.42845, "y":0.5944, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.76244, "ay":-2.50922, "alpha":14.64713, "fx":[246.89752,206.36174,234.13418,248.98753], "fy":[36.7298,-140.56369,-87.13767,20.24743]}, + {"t":0.03921, "x":4.43903, "y":0.59247, "heading":0.01126, "vx":0.53968, "vy":-0.0984, "omega":0.57438, "ax":13.75332, "ay":-2.41865, "alpha":12.82178, "fx":[247.94095,211.75588,227.76447,248.29879], "fy":[28.13967,-132.15982,-79.69258,19.15054]}, + {"t":0.07843, "x":4.47077, "y":0.58675, "heading":0.04364, "vx":1.07901, "vy":-0.19324, "omega":1.07717, "ax":12.43109, "ay":-1.97263, "alpha":9.83486, "fx":[245.36853,205.23477,187.75049,207.44344], "fy":[44.7894,-137.00661,-63.71607,21.71783]}, + {"t":0.11764, "x":4.52264, "y":0.57765, "heading":0.09345, "vx":1.56649, "vy":-0.2706, "omega":1.46284, "ax":11.15688, "ay":-1.72289, "alpha":6.61268, "fx":[239.29626,179.20406,160.76248,179.83852], "fy":[31.23356,-118.60623,-51.28291,21.43206]}, + {"t":0.15686, "x":4.59264, "y":0.56572, "heading":0.15589, "vx":2.00399, "vy":-0.33816, "omega":1.72215, "ax":10.05279, "ay":-1.53333, "alpha":4.19205, "fx":[218.73319,162.4703,141.99224,160.78473], "fy":[15.22933,-97.44886,-40.66552,18.55888]}, + {"t":0.19607, "x":4.67896, "y":0.55128, "heading":0.22665, "vx":2.39821, "vy":-0.39829, "omega":1.88654, "ax":9.17295, "ay":-1.38394, "alpha":2.34866, "fx":[200.1595,149.01737,128.05821,146.88191], "fy":[1.73791,-78.91506,-32.29106,15.30674]}, + {"t":0.23529, "x":4.78005, "y":0.5346, "heading":0.30244, "vx":2.75792, "vy":-0.45256, "omega":1.97864, "ax":8.44832, "ay":-1.24854, "alpha":0.89356, "fx":[183.03393,138.09709,117.35389,136.32914], "fy":[-8.57651,-62.00678,-25.44302,11.07675]}, + {"t":0.2745, "x":4.8947, "y":0.51589, "heading":0.38071, "vx":3.08921, "vy":-0.50152, "omega":2.01368, "ax":7.65472, "ay":-1.06915, "alpha":-1.28945, "fx":[166.93361,129.56117,96.04538,128.27835], "fy":[-15.32815,-44.84799,-17.09851,4.53074]}, + {"t":0.31371, "x":5.02173, "y":0.4954, "heading":0.45869, "vx":3.38939, "vy":-0.54344, "omega":1.96311, "ax":6.46266, "ay":-0.82524, "alpha":-3.68143, "fx":[152.90344,119.39283,45.37919,122.03622], "fy":[-13.98312,-42.3401,-7.34019,7.51531]}, + {"t":0.35293, "x":5.15961, "y":0.47345, "heading":0.53284, "vx":3.64281, "vy":-0.57581, "omega":1.81875, "ax":5.55276, "ay":-0.6208, "alpha":-5.07185, "fx":[140.54522,111.15563,9.79199,116.31042], "fy":[-11.46391,-40.93114,-1.39547,11.5517]}, + {"t":0.39214, "x":5.30673, "y":0.4504, "heading":0.60026, "vx":3.86056, "vy":-0.60015, "omega":1.61986, "ax":4.80352, "ay":-0.39404, "alpha":-5.77465, "fx":[129.52306,102.40136,-15.00313,109.90482], "fy":[-7.2502,-39.02596,2.71632,16.74984]}, + {"t":0.43136, "x":5.46181, "y":0.42656, "heading":0.65934, "vx":4.04893, "vy":-0.6156, "omega":1.39341, "ax":3.63517, "ay":-0.08546, "alpha":-6.46929, "fx":[119.6738,67.98959,-25.18232,84.85146], "fy":[-2.17609,-26.51326,4.80487,18.06998]}, + {"t":0.47057, "x":5.62338, "y":0.40235, "heading":0.70901, "vx":4.19148, "vy":-0.61895, "omega":1.13972, "ax":2.75622, "ay":0.14032, "alpha":-6.55212, "fx":[109.77049,43.84972,-26.52118,60.43085], "fy":[4.40781,-17.81045,5.44235,17.50757]}, + {"t":0.50979, "x":5.78986, "y":0.37819, "heading":0.74866, "vx":4.29956, "vy":-0.61345, "omega":0.88279, "ax":1.91009, "ay":0.38604, "alpha":-5.35634, "fx":[86.11942,28.29448,-26.12999,41.67673], "fy":[16.53967,-12.94428,5.6476,17.02305]}, + {"t":0.549, "x":5.95994, "y":0.35443, "heading":0.77916, "vx":4.37446, "vy":-0.59831, "omega":0.67274, "ax":1.24193, "ay":0.46756, "alpha":-3.80968, "fx":[57.83966,19.70607,-23.33028,30.28376], "fy":[20.78225,-8.72359,5.31987,14.43357]}, + {"t":0.58821, "x":6.13243, "y":0.33133, "heading":0.80262, "vx":4.42316, "vy":-0.57998, "omega":0.52335, "ax":0.80972, "ay":0.47973, "alpha":-2.62298, "fx":[37.67703,14.50669,-20.08077,22.98919], "fy":[21.60429,-6.06952,4.81732,12.28799]}, + {"t":0.62743, "x":6.30651, "y":0.30895, "heading":0.82112, "vx":4.45492, "vy":-0.56117, "omega":0.42049, "ax":0.54641, "ay":0.47133, "alpha":-1.85056, "fx":[25.0004,11.2236,-17.26369,18.21683], "fy":[21.51596,-4.39398,4.33079,10.61588]}, + {"t":0.66664, "x":6.48162, "y":0.28731, "heading":0.83619, "vx":4.47634, "vy":-0.54268, "omega":0.34792, "ax":0.13066, "ay":0.97406, "alpha":-0.05529, "fx":[-5.27157,10.84599,-15.44403,18.75986], "fy":[56.3281,-3.22219,3.99559,9.17218]}, + {"t":0.70586, "x":6.65726, "y":0.26678, "heading":0.84979, "vx":4.48147, "vy":-0.50449, "omega":0.34575, "ax":0.10672, "ay":1.04794, "alpha":-0.01588, "fx":[-7.22385,10.91432,-15.36925,18.93997], "fy":[60.77902,-2.79341,4.02767,9.28719]}, + {"t":0.74507, "x":6.83308, "y":0.2478, "heading":0.86334, "vx":4.48565, "vy":-0.46339, "omega":0.34513, "ax":-1.09181, "ay":2.85773, "alpha":7.22633, "fx":[48.69637,12.0365,-29.77344,-105.24473], "fy":[9.86697,-1.22845,7.46365,178.33452]}, + {"t":0.78428, "x":7.00814, "y":0.23183, "heading":0.88243, "vx":4.44284, "vy":-0.35133, "omega":0.62851, "ax":-0.38382, "ay":2.8069, "alpha":3.07126, "fx":[84.38944,19.08674,-36.94585,-92.64511], "fy":[23.47704,-0.48041,9.81508,158.16637]}, + {"t":0.8235, "x":7.18207, "y":0.22021, "heading":0.90943, "vx":4.42779, "vy":-0.24126, "omega":0.74894, "ax":-0.72702, "ay":3.46385, "alpha":3.9022, "fx":[94.35331,21.52693,-45.34649,-119.99954], "fy":[35.31098,1.39204,13.03621,185.93709]}, + {"t":0.86271, "x":7.35514, "y":0.21341, "heading":0.9418, "vx":4.39928, "vy":-0.10543, "omega":0.90197, "ax":-0.82606, "ay":3.78343, "alpha":1.88384, "fx":[-84.94128,26.46766,-45.14399,47.41346], "fy":[217.97755,1.37831,11.69081,26.37324]}, + {"t":0.90193, "x":7.52702, "y":0.21219, "heading":0.97862, "vx":4.36688, "vy":0.04294, "omega":0.97584, "ax":-0.79186, "ay":3.93588, "alpha":1.86521, "fx":[-93.36828,31.76304,-46.71265,54.4405], "fy":[218.48696,5.10649,12.06067,32.13837]}, + {"t":0.94114, "x":7.69766, "y":0.2169, "heading":1.01832, "vx":4.33583, "vy":0.19728, "omega":1.04898, "ax":-4.11035, "ay":6.42703, "alpha":13.16686, "fx":[-103.58138,34.2524,-68.94039,-141.39383], "fy":[222.2478,9.91084,17.46094,187.66839]}, + {"t":0.98036, "x":7.86452, "y":0.22957, "heading":1.06958, "vx":4.17465, "vy":0.44931, "omega":1.56531, "ax":-4.67999, "ay":6.51717, "alpha":11.55487, "fx":[-110.84692,50.83951,-90.59857,-167.81523], "fy":[221.478,20.47583,24.8968,176.57033]}, + {"t":1.01957, "x":8.02463, "y":0.2522, "heading":1.13985, "vx":3.99113, "vy":0.70488, "omega":2.01843, "ax":-5.54812, "ay":6.81776, "alpha":8.74812, "fx":[-118.78001,62.41642,-132.17892,-188.94534], "fy":[218.38774,35.02008,51.76129,158.70376]}, + {"t":1.05878, "x":8.17687, "y":0.28509, "heading":1.22573, "vx":3.77356, "vy":0.97223, "omega":2.36148, "ax":-7.0985, "ay":7.45386, "alpha":4.49688, "fx":[-131.09549,68.74899,-214.08849,-206.53891], "fy":[211.73105,56.50971,101.8694,137.04173]}, + {"t":1.098, "x":8.31939, "y":0.32894, "heading":1.32179, "vx":3.4952, "vy":1.26453, "omega":2.53782, "ax":-10.63488, "ay":8.83749, "alpha":-7.19699, "fx":[-159.01888,-122.25958,-221.88693,-220.41982], "fy":[192.08224,187.234,106.48275,115.49366]}, + {"t":1.13721, "x":8.44828, "y":0.38533, "heading":1.41577, "vx":3.07816, "vy":1.61108, "omega":2.25559, "ax":-11.77217, "ay":8.19891, "alpha":-5.57198, "fx":[-186.84491,-167.27244,-213.51512,-233.33266], "fy":[165.28775,182.44103,123.33581,86.77994]}, + {"t":1.17643, "x":8.55993, "y":0.45481, "heading":1.49994, "vx":2.61652, "vy":1.9326, "omega":2.03709, "ax":-9.52768, "ay":7.33077, "alpha":-11.7382, "fx":[-213.13676,-196.75039,-226.85553,-11.51003], "fy":[129.68095,152.45068,100.00736,116.63805]}, + {"t":1.21564, "x":8.65521, "y":0.53623, "heading":1.5708, "vx":2.2429, "vy":2.22007, "omega":1.57679, "ax":-10.42315, "ay":5.64656, "alpha":-10.13488, "fx":[-234.80313,-223.73354,-238.35389,-12.28843], "fy":[84.04491,109.37122,68.75027,122.01927]}, + {"t":1.26226, "x":8.74845, "y":0.64587, "heading":1.63329, "vx":1.75697, "vy":2.48331, "omega":1.1043, "ax":-7.80605, "ay":5.02611, "alpha":-21.16433, "fx":[-28.61356,-243.52966,-243.04293,-15.92849], "fy":[126.63727,45.33769,41.35182,128.64367]}, + {"t":1.30888, "x":8.82188, "y":0.7671, "heading":1.66178, "vx":1.39305, "vy":2.71763, "omega":0.11761, "ax":-4.51992, "ay":5.68507, "alpha":-14.78535, "fx":[-20.94065,-18.29731,-247.18626,-21.10618], "fy":[118.4632,128.38048,13.46911,126.49318]}, + {"t":1.3555, "x":8.88191, "y":0.89997, "heading":1.65119, "vx":1.18233, "vy":2.98267, "omega":-0.57168, "ax":-1.41163, "ay":6.91326, "alpha":0.49529, "fx":[-21.46811,-24.30368,-26.3495,-23.92408], "fy":[111.27484,116.61118,124.5976,117.88675]}, + {"t":1.40212, "x":8.9355, "y":1.04654, "heading":1.62508, "vx":1.11652, "vy":3.30497, "omega":-0.54859, "ax":-1.52395, "ay":6.42281, "alpha":0.60196, "fx":[-24.58625,-25.7774,-27.14152,-26.18239], "fy":[103.25986,108.0612,115.74593,109.93381]}, + {"t":1.44874, "x":8.98589, "y":1.2076, "heading":1.60016, "vx":1.04548, "vy":3.6044, "omega":-0.52053, "ax":-1.84911, "ay":5.78677, "alpha":0.90116, "fx":[-30.97754,-30.75262,-31.31576,-32.76549], "fy":[89.92754,98.33847,105.73206,99.72727]}, + {"t":1.49536, "x":9.03262, "y":1.38192, "heading":1.57687, "vx":0.95927, "vy":3.87418, "omega":-0.47851, "ax":-4.52027, "ay":3.61161, "alpha":11.63902, "fx":[-191.97393,-35.17496,-48.84527,-31.56006], "fy":[-26.9804,94.50373,81.19808,97.00855]}, + {"t":1.54198, "x":9.07243, "y":1.56646, "heading":1.56721, "vx":0.74854, "vy":4.04255, "omega":0.0641, "ax":-5.5062, "ay":2.20314, "alpha":0.01163, "fx":[-42.41495,-47.41071,-50.10417,-234.70589], "fy":[85.33039,78.70302,64.36264,-78.4971]}, + {"t":1.5886, "x":9.10135, "y":1.75732, "heading":1.57021, "vx":0.49184, "vy":4.14526, "omega":0.06464, "ax":-3.84406, "ay":1.94092, "alpha":-7.75382, "fx":[-33.70439,-26.33125,-161.92161,-39.58793], "fy":[39.34141,52.45099,-37.79938,78.06483]}, + {"t":1.63522, "x":9.1201, "y":1.95268, "heading":1.5648, "vx":0.31263, "vy":4.23575, "omega":-0.29684, "ax":-1.61686, "ay":2.43284, "alpha":1.56358, "fx":[-17.7255,-19.796,-37.88463,-34.60298], "fy":[31.78323,31.41696,50.30741,52.0201]}, + {"t":1.68184, "x":9.13292, "y":2.1528, "heading":1.55266, "vx":0.23725, "vy":4.34917, "omega":-0.22395, "ax":-1.02907, "ay":1.68908, "alpha":1.21663, "fx":[-10.45903,-11.92103,-25.17093,-22.46609], "fy":[21.44249,20.66853,35.26921,37.54258]}, + {"t":1.72846, "x":9.14286, "y":2.35739, "heading":1.54354, "vx":0.18927, "vy":4.42791, "omega":-0.16723, "ax":-0.65216, "ay":1.14322, "alpha":0.92268, "fx":[-6.00392,-7.0509,-16.77791,-14.53959], "fy":[13.95451,13.17593,24.20803,26.4449]}, + {"t":1.77508, "x":9.15097, "y":2.56507, "heading":1.53675, "vx":0.15887, "vy":4.48121, "omega":-0.12421, "ax":-0.41084, "ay":0.76099, "alpha":0.68298, "fx":[-3.27692,-4.06471,-11.29325,-9.31843], "fy":[8.82134,8.16859,16.35379,18.43293]}, + {"t":1.8217, "x":9.15793, "y":2.77481, "heading":1.5317, "vx":0.13971, "vy":4.51669, "omega":-0.09237, "ax":-0.87439, "ay":0.12133, "alpha":-0.74948, "fx":[-11.50254,-2.08429,-38.41747,-7.48813], "fy":[-4.23721,5.48851,-13.06222,20.06638]}, + {"t":1.86832, "x":9.1635, "y":2.98551, "heading":1.52658, "vx":0.09895, "vy":4.52234, "omega":-0.12731, "ax":-0.78898, "ay":0.11529, "alpha":-0.66044, "fx":[-6.97283,-0.90118,-37.73694,-8.07063], "fy":[-3.31421,2.31534,-12.34395,21.18707]}, + {"t":1.91494, "x":9.16725, "y":3.19646, "heading":1.51992, "vx":0.06217, "vy":4.52772, "omega":-0.1581, "ax":-0.79217, "ay":0.13611, "alpha":0.86637, "fx":[-0.56573,-36.11512,-10.86034,-6.35728], "fy":[3.35847,-26.68162,13.79604,18.78762]}, + {"t":1.96156, "x":9.16929, "y":3.4077, "heading":1.51349, "vx":0.02524, "vy":4.53407, "omega":-0.11771, "ax":-0.59306, "ay":0.10702, "alpha":-0.85731, "fx":[-0.33337,-0.75625,-32.30201,-6.95975], "fy":[0.97765,-1.17279,-11.24449,18.72133]}, + {"t":2.00818, "x":9.16982, "y":3.61919, "heading":1.50707, "vx":-0.00241, "vy":4.53905, "omega":-0.15768, "ax":-0.66883, "ay":0.02028, "alpha":-0.66395, "fx":[-0.81194,-2.74241,-34.52264,-7.42974], "fy":[-1.78358,-4.96865,-12.38263,20.51443]}, + {"t":2.0548, "x":9.16898, "y":3.83082, "heading":1.499, "vx":-0.03359, "vy":4.54, "omega":-0.18863, "ax":-0.7512, "ay":-0.03626, "alpha":-0.55809, "fx":[-1.47301,-4.04817,-37.3123,-8.2773], "fy":[-3.33363,-7.67396,-13.9162,22.45665]}, + {"t":2.10142, "x":9.1666, "y":4.04244, "heading":1.4896, "vx":-0.06861, "vy":4.53831, "omega":-0.21465, "ax":-0.83804, "ay":-0.07987, "alpha":-0.53895, "fx":[-1.87276,-5.05281,-40.74963,-9.34408], "fy":[-4.2319,-9.8489,-15.96527,24.6118]}, + {"t":2.14804, "x":9.16249, "y":4.25393, "heading":1.47901, "vx":-0.10768, "vy":4.53459, "omega":-0.23978, "ax":-0.93681, "ay":-0.11917, "alpha":-0.5541, "fx":[-2.21312,-6.14159,-44.66067,-10.72428], "fy":[-4.76895,-11.98422,-18.39523,27.04022]}, + {"t":2.19466, "x":9.15645, "y":4.4652, "heading":1.46723, "vx":-0.15136, "vy":4.52903, "omega":-0.26561, "ax":-1.05391, "ay":-0.16066, "alpha":-0.55931, "fx":[-2.6903,-7.73836,-48.78258,-12.49528], "fy":[-5.13739,-14.47672,-21.04485,29.72767]}, + {"t":2.24128, "x":9.14825, "y":4.67617, "heading":1.45424, "vx":-0.20049, "vy":4.52154, "omega":-0.29169, "ax":-1.21434, "ay":-0.22348, "alpha":-0.48219, "fx":[-4.15032,-10.8458,-52.94266,-14.68353], "fy":[-5.95922,-17.98027,-23.86146,32.59546]}, + {"t":2.2879, "x":9.13758, "y":4.88672, "heading":1.44011, "vx":-0.2571, "vy":4.51112, "omega":-0.31417, "ax":-1.53849, "ay":-0.40174, "alpha":-0.24263, "fx":[-13.30155,-12.42413,-62.0207,-16.93069], "fy":[-12.57622,-19.21001,-31.41209,35.86472]}, + {"t":2.33452, "x":9.12393, "y":5.0966, "heading":1.4252, "vx":-0.32883, "vy":4.49239, "omega":-0.32548, "ax":-3.33088, "ay":-1.8308, "alpha":4.01168, "fx":[-98.23671,-10.39162,-98.35762,-19.64344], "fy":[-83.64209,-15.59615,-65.10919,39.78215]}, + {"t":2.38114, "x":9.10498, "y":5.30404, "heading":1.41439, "vx":-0.48411, "vy":4.40704, "omega":-0.13845, "ax":-3.33305, "ay":-0.81833, "alpha":-1.5143, "fx":[-15.33519,-11.64083,-20.01317,-179.78751], "fy":[24.0657,19.32042,37.45251,-136.5166]}, + {"t":2.42777, "x":9.07878, "y":5.50861, "heading":1.40629, "vx":-0.6395, "vy":4.36889, "omega":-0.20905, "ax":-10.07705, "ay":-8.55244, "alpha":-0.19129, "fx":[-161.82207,-158.02586,-181.29112,-184.49193], "fy":[-138.09423,-149.11527,-153.71136,-140.97716]}, + {"t":2.47439, "x":9.03802, "y":5.703, "heading":1.39634, "vx":-1.1093, "vy":3.97017, "omega":-0.21797, "ax":-8.4208, "ay":-5.98899, "alpha":11.84077, "fx":[-152.55182,-176.64266,-73.92256,-169.82429], "fy":[-142.97411,-165.69786,71.27552,-170.08788]}, + {"t":2.52101, "x":8.97715, "y":5.88158, "heading":1.39904, "vx":-1.50187, "vy":3.69097, "omega":0.33405, "ax":-7.93017, "ay":-8.17507, "alpha":10.7038, "fx":[-138.92599,-163.39073,-88.56792,-148.67507], "fy":[-205.72718,-187.39954,35.1622,-198.25781]}, + {"t":2.56763, "x":8.89852, "y":6.04477, "heading":1.42625, "vx":-1.87158, "vy":3.30984, "omega":0.83306, "ax":-7.20748, "ay":-8.8247, "alpha":8.61039, "fx":[-101.50776,-141.60307,-156.73114,-90.54651], "fy":[-227.55242,-205.23865,-193.14282,25.51138]}, + {"t":2.61425, "x":8.80343, "y":6.18948, "heading":1.47444, "vx":-2.20759, "vy":2.89843, "omega":1.23448, "ax":-6.00284, "ay":-9.66585, "alpha":8.24549, "fx":[-58.87306,-115.32312,-141.30905,-92.92107], "fy":[-242.32435,-221.33159,-205.13965,11.14239]}, + {"t":2.66087, "x":8.69399, "y":6.3141, "heading":1.54095, "vx":-2.48745, "vy":2.44781, "omega":1.61889, "ax":-5.07744, "ay":-10.05797, "alpha":8.13939, "fx":[-27.06912,-94.03188,-129.19102,-95.17115], "fy":[-247.58571,-231.01644,-212.67641,6.94599]}, + {"t":2.69295, "x":8.61157, "y":6.38746, "heading":1.59708, "vx":-2.65035, "vy":2.12511, "omega":1.88003, "ax":-4.11796, "ay":-10.28738, "alpha":8.14865, "fx":[5.42017,-71.79616,-117.11468,-96.69079], "fy":[-248.49347,-238.72608,-219.335,6.61271]}, + {"t":2.72503, "x":8.52442, "y":6.45035, "heading":1.6616, "vx":-2.78247, "vy":1.79505, "omega":2.14148, "ax":-3.10448, "ay":-10.41502, "alpha":7.95631, "fx":[37.51906,-48.15493,-104.53853,-96.05119], "fy":[-244.5926,-244.43544,-225.19346,5.59576]}, + {"t":2.75712, "x":8.43354, "y":6.50258, "heading":1.7344, "vx":-2.88208, "vy":1.46089, "omega":2.39675, "ax":-4.47669, "ay":-7.62727, "alpha":3.27158, "fx":[-111.03453,-25.58186,-86.43235,-81.53992], "fy":[-40.93871,-247.64408,-232.16447,1.79663]}, + {"t":2.7892, "x":8.33877, "y":6.54553, "heading":1.81298, "vx":-3.02571, "vy":1.21618, "omega":2.50171, "ax":-3.71665, "ay":-7.73465, "alpha":2.66248, "fx":[-112.26475,-4.38793,-68.0562,-68.16802], "fy":[-42.35905,-248.52177,-236.10266,0.72684]}, + {"t":2.82129, "x":8.23978, "y":6.58057, "heading":1.89462, "vx":-3.14495, "vy":0.96802, "omega":2.58714, "ax":-3.60087, "ay":-4.14811, "alpha":11.40494, "fx":[-110.96977,2.33143,-121.78075,-14.5798], "fy":[-46.23379,-248.14111,12.31094,-0.1687]}, + {"t":2.85337, "x":8.13702, "y":6.60949, "heading":1.98349, "vx":-3.26049, "vy":0.83493, "omega":2.95305, "ax":-2.86927, "ay":-4.22485, "alpha":8.51155, "fx":[-108.65323,5.58969,-119.95753,27.79908], "fy":[-49.448,-247.78057,25.05203,-15.27752]}, + {"t":2.88545, "x":8.03094, "y":6.6341, "heading":2.08262, "vx":-3.35254, "vy":0.69938, "omega":3.22614, "ax":-2.30229, "ay":-4.35206, "alpha":5.79081, "fx":[-106.19342,7.03732,-116.69544,59.20626], "fy":[-54.02504,-247.33805,29.9418,-24.6876]}, + {"t":2.91754, "x":7.92219, "y":6.6543, "heading":2.18911, "vx":-3.42641, "vy":0.55975, "omega":3.41193, "ax":-1.88644, "ay":-4.39433, "alpha":3.53189, "fx":[-103.81075,7.63148,-113.32769,81.15577], "fy":[-59.60534,-246.8477,32.502,-25.03439]}, + {"t":2.94962, "x":7.81128, "y":6.67, "heading":2.3004, "vx":-3.48694, "vy":0.41876, "omega":3.52525, "ax":-1.58733, "ay":-4.41035, "alpha":1.61359, "fx":[-101.22264,8.52203,-110.12694,94.8273], "fy":[-65.9619,-246.20177,32.78499,-20.69615]}, + {"t":2.98171, "x":7.69859, "y":6.68116, "heading":2.41433, "vx":-3.53786, "vy":0.27725, "omega":3.57702, "ax":-1.34855, "ay":-4.43762, "alpha":0.06197, "fx":[-97.96986,11.56998,-106.92571,101.57194], "fy":[-72.93688,-245.18086,30.90007,-14.71319]}, + {"t":3.01379, "x":7.58439, "y":6.68778, "heading":2.52913, "vx":-3.58113, "vy":0.13488, "omega":3.57901, "ax":-0.86353, "ay":-4.53355, "alpha":-1.0903, "fx":[-92.89959,20.16768,-87.4707,101.44914], "fy":[-80.84954,-243.3168,25.38549,-9.67655]}, + {"t":3.04588, "x":7.46905, "y":6.68977, "heading":2.6434, "vx":-3.60884, "vy":-0.01058, "omega":3.54403, "ax":0.03573, "ay":-4.4709, "alpha":-0.27742, "fx":[-85.97444,56.06761,-66.86151,99.19956], "fy":[-89.35549,-232.12102,22.15205,-4.8702]}, + {"t":3.07796, "x":7.35328, "y":6.68713, "heading":2.75696, "vx":-3.60769, "vy":-0.15402, "omega":3.53513, "ax":1.17292, "ay":-2.0038, "alpha":5.0191, "fx":[-79.84358,106.73323,-39.59668,92.51134], "fy":[-97.1396,-54.47027,12.65327,2.62064]}, + {"t":3.11004, "x":7.23813, "y":6.68116, "heading":2.87297, "vx":-3.57006, "vy":-0.21831, "omega":3.69616, "ax":2.76274, "ay":0.87453, "alpha":13.13135, "fx":[-71.02887,167.16248,2.36863,89.47173], "fy":[-107.39356,159.69998,-0.44234,7.63827]}, + {"t":3.14213, "x":7.12501, "y":6.6746, "heading":2.99831, "vx":-3.48142, "vy":-0.19025, "omega":4.11747, "ax":3.82401, "ay":0.64719, "alpha":13.46353, "fx":[-47.2058,175.36769,43.1609,88.85872], "fy":[-123.65164,167.63736,-7.22226,7.2706]}, + {"t":3.17421, "x":7.01528, "y":6.66883, "heading":3.13735, "vx":-3.35873, "vy":-0.16949, "omega":4.54944, "ax":8.16693, "ay":-0.17067, "alpha":23.55416, "fx":[198.36397,202.10644,81.88231,73.31585], "fy":[-142.3442,136.27366,-11.83296,6.29109]}, + {"t":3.2063, "x":6.91172, "y":6.66331, "heading":-2.98775, "vx":-3.0967, "vy":-0.17497, "omega":5.30515, "ax":12.22789, "ay":-1.40746, "alpha":17.02739, "fx":[232.0784,238.81643,140.64377,220.43318], "fy":[-80.83432,55.36634,17.71716,-88.01147]}, + {"t":3.23838, "x":6.81866, "y":6.65697, "heading":-2.80877, "vx":-2.70438, "vy":-0.22012, "omega":5.85146, "ax":13.74838, "ay":0.12535, "alpha":-15.68953, "fx":[233.14657,220.74469,245.69033,235.84255], "fy":[79.31189,-111.64791,-36.3006,77.16545]}, + {"t":3.27046, "x":6.73897, "y":6.64997, "heading":-2.62911, "vx":-2.26327, "vy":-0.2161, "omega":5.34808, "ax":12.21801, "ay":1.84635, "alpha":-26.45149, "fx":[181.48108,192.61595,248.48367,208.71874], "fy":[166.80174,-157.37518,-19.78979,135.98663]}, + {"t":3.30255, "x":6.67264, "y":6.64399, "heading":-2.47114, "vx":-1.87127, "vy":-0.15686, "omega":4.4994, "ax":11.14466, "ay":3.06416, "alpha":-30.62644, "fx":[133.03716,188.6766,249.54828,187.00759], "fy":[206.77704,-162.8797,-0.44033,165.02475]}, + {"t":3.33463, "x":6.61834, "y":6.64053, "heading":-2.34254, "vx":-1.5137, "vy":-0.05855, "omega":3.51678, "ax":11.70334, "ay":2.69827, "alpha":-26.63281, "fx":[183.273,193.11491,249.1685,170.72533], "fy":[143.57477,-157.97949,15.97801,182.01395]}, + {"t":3.36672, "x":6.5758, "y":6.64004, "heading":-2.24341, "vx":-1.13821, "vy":0.02802, "omega":2.66229, "ax":12.49188, "ay":1.02142, "alpha":-23.22258, "fx":[238.05243,202.56097,247.92252,161.39718], "fy":[-5.25255,-145.89253,30.19566,190.44572]}, + {"t":3.3988, "x":6.54571, "y":6.64147, "heading":-2.16995, "vx":-0.73742, "vy":0.06079, "omega":1.91721, "ax":11.8687, "ay":-0.65852, "alpha":-27.63496, "fx":[199.71006,207.10446,246.71183,154.00654], "fy":[-141.01163,-139.51476,39.17965,196.54194]}, + {"t":3.43089, "x":6.52816, "y":6.64308, "heading":-2.12266, "vx":-0.35662, "vy":0.03966, "omega":1.03057, "ax":11.11512, "ay":-1.23624, "alpha":-32.12072, "fx":[152.90672,209.26063,245.79317,148.29956], "fy":[-193.46706,-136.35991,44.7766,200.93783]}, + {"t":3.46297, "x":6.52244, "y":6.64371, "heading":-2.10613, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.00277, "ay":0.00045, "alpha":0.00409, "fx":[-0.03958,-0.04687,-0.05497,-0.0471], "fy":[0.0205,0.0264,0.00106,-0.01744]}], + "splits":[0] + }, + "events":[] +} From 6db5b16d9c481888cc1982e8af5d75ce783789b2 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 06:13:57 -0400 Subject: [PATCH 21/53] global variant/dt tables --- .github/workflows/benchmark.yml | 4 +- scripts/bench-report.mjs | 103 ++++++++++++++++++++++ test-projects/swerve-base/project.chor | 110 ------------------------ test-projects/swerve-fast/project.chor | 110 ------------------------ test-projects/swerve-heavy/project.chor | 110 ------------------------ test-projects/swerve-weak/project.chor | 110 ------------------------ 6 files changed, 105 insertions(+), 442 deletions(-) delete mode 100755 test-projects/swerve-base/project.chor delete mode 100755 test-projects/swerve-fast/project.chor delete mode 100755 test-projects/swerve-heavy/project.chor delete mode 100755 test-projects/swerve-weak/project.chor diff --git a/.github/workflows/benchmark.yml b/.github/workflows/benchmark.yml index 308b10ad4e..3f8c2e6899 100644 --- a/.github/workflows/benchmark.yml +++ b/.github/workflows/benchmark.yml @@ -182,13 +182,13 @@ jobs: - name: Run PR benchmark continue-on-error: true - timeout-minutes: 30 + timeout-minutes: 60 run: bash scripts/run-bench.sh "$PWD/bin-pr/choreo-cli" tp-pr out-pr "$VARIANT" - name: Run base benchmark continue-on-error: true # tolerate missing / schema-incompatible base - timeout-minutes: 30 + timeout-minutes: 60 run: | if [ -x bin-base/choreo-cli ]; then bash scripts/run-bench.sh "$PWD/bin-base/choreo-cli" tp-base out-base "$VARIANT" diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index 59b696b48f..c74e16a36e 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -193,6 +193,10 @@ function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { L.push(`${summary.comparable} comparable · ${summary.prFailed} failed on PR · ${summary.baseFailed} failed on base · ${summary.flaky} flaky · ${summary.total} total`); L.push(""); + // Overarching cross-dt tables (above the per-variant dropdowns). No-op when + // no variant follows the `--dt` scheme. + L.push(...dtSummaryTables(rows)); + // Group rows by variant (listVariants order is preserved by push order). const byVariant = new Map(); for (const r of rows) { @@ -346,6 +350,105 @@ function median(xs) { return s.length % 2 ? s[m] : (s[m - 1] + s[m]) / 2; } +// Variants named `--dt` (e.g. swerve-base-extra-low-dt) feed the +// cross-dt summary tables. +function parseDt(variant) { + if (typeof variant !== "string" || !variant.endsWith("-dt")) return null; + const stem = variant.slice(0, -3); // strip trailing "-dt" + // Test longest-first so the "-low" suffix can't steal "-extra-low". + for (const tier of ["extra-low", "medium", "high", "low"]) { + if (stem.endsWith(`-${tier}`)) { + return { family: stem.slice(0, -(tier.length + 1)), tier }; + } + } + return null; +} + +// Two overarching tables comparing the dt tiers. Returns [] (no-op) when no +// variant follows the scheme, so non-dt corpora are unaffected. +function dtSummaryTables(rows) { + // Magnitude order — used for COLUMN order so the sweep reads low→high dt + // (plain alphabetical would wrongly put "high" 2nd). Local const: this runs + // via renderMarkdown at module-eval time, before a top-level const would init. + const DT_TIERS = ["extra-low", "low", "medium", "high"]; + const tiersSeen = new Set(); + const families = new Set(); + const trajNames = new Set(); + const byFamilyTier = new Map(); // `${family}${tier}` -> rows[] + const byCell = new Map(); // `${family}${traj}${tier}` -> row + + for (const r of rows) { + const d = parseDt(r.variant); + if (!d) continue; + families.add(d.family); + tiersSeen.add(d.tier); + if (r.noReport) continue; // counts toward family/tier, but has no rows + trajNames.add(r.name); + const ft = `${d.family}${d.tier}`; + if (!byFamilyTier.has(ft)) byFamilyTier.set(ft, []); + byFamilyTier.get(ft).push(r); + byCell.set(`${d.family}${r.name}${d.tier}`, r); + } + + if (tiersSeen.size === 0) return []; + + const tiers = DT_TIERS.filter(t => tiersSeen.has(t)); // present, magnitude order + const fams = [...families].sort(); + const trajs = [...trajNames].sort(); + const alignNum = tiers.map(() => "---:").join("|"); + + const L = []; + L.push(`#### dt sweep`); + L.push(""); + + // Table 1 — per-family rollup: Σ PR solve over the family's trajectories. + L.push(`Total PR solve per family across dt tiers (sum over that family's trajectories; lower = faster).`); + L.push(""); + L.push(`| Family | ${tiers.join(" | ")} |`); + L.push(`|---|${alignNum}|`); + let anyPartial = false; + for (const fam of fams) { + const cells = tiers.map(t => { + const rs = byFamilyTier.get(`${fam}${t}`); + if (!rs || rs.length === 0) return "—"; + const ok = rs.filter(r => r.prOk && r.pr && Number.isFinite(r.pr.mean)); + if (ok.length === 0) return "—"; + const sum = ok.reduce((a, r) => a + r.pr.mean, 0); + const partial = ok.length < trajs.length; + if (partial) anyPartial = true; + return `${fmtTotal(sum)}${partial ? ` \\*${ok.length}/${trajs.length}` : ""}`; + }); + L.push(`| \`${fam}\` | ${cells.join(" | ")} |`); + } + if (anyPartial) { + L.push(""); + L.push(`\\* partial — some trajectories failed on PR for that cell, so its total covers fewer trajectories and is not like-for-like across tiers.`); + } + L.push(""); + + // Table 2 — regression across dt: PR-vs-base Δ% per family × trajectory. + L.push(`PR-vs-base solve Δ% per trajectory across dt tiers (is a regression consistent, or dt-dependent?). Bold = |Δ| > ${SOLVE_PCT_THRESHOLD}%.`); + L.push(""); + L.push(`| Family | Trajectory | ${tiers.join(" | ")} |`); + L.push(`|---|---|${alignNum}|`); + for (const fam of fams) { + for (const tj of trajs) { + const cells = tiers.map(t => { + const r = byCell.get(`${fam}${tj}${t}`); + if (!r) return "—"; + if (!r.prOk) return "PR✗"; + if (!r.baseOk) return "base✗"; + if (r.pct == null) return "~"; + const txt = signedPct(r.pct); + return Math.abs(r.pct) > SOLVE_PCT_THRESHOLD ? `**${txt}**` : txt; + }); + L.push(`| \`${fam}\` | \`${tj}\` | ${cells.join(" | ")} |`); + } + } + L.push(""); + return L; +} + function parseArgs(args) { const out = {}; for (let i = 0; i < args.length; i++) { diff --git a/test-projects/swerve-base/project.chor b/test-projects/swerve-base/project.chor deleted file mode 100755 index 1fd8d31d68..0000000000 --- a/test-projects/swerve-base/project.chor +++ /dev/null @@ -1,110 +0,0 @@ -{ - "name":"project", - "version":3, - "type":"Swerve", - "variables":{ - "expressions":{}, - "poses":{} - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"11 in", - "val":0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "backLeft":{ - "x":{ - "exp":"-11 in", - "val":-0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "mass":{ - "exp":"125 lbs", - "val":56.69904625 - }, - "inertia":{ - "exp":"5 kg m ^ 2", - "val":5.0 - }, - "gearing":{ - "exp":"6.5", - "val":6.5 - }, - "radius":{ - "exp":"2 in", - "val":0.0508 - }, - "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 - }, - "tmax":{ - "exp":"1.2 N * m", - "val":1.2 - }, - "cof":{ - "exp":"2.0", - "val":2.0 - }, - "bumper":{ - "front":{ - "exp":"16 in", - "val":0.4064 - }, - "side":{ - "exp":"16 in", - "val":0.4064 - }, - "back":{ - "exp":"16 in", - "val":0.4064 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - }, - "motorConfig":{ - "free_speed":{ - "exp":"5800 RPM", - "val":607.3745796940267 - }, - "stall_torque":{ - "exp":"9.36 N * m", - "val":9.36 - }, - "kT":{ - "exp":"0.02 N * m / A", - "val":0.02 - }, - "kV":{ - "exp":"0.00206896552 V / rpm", - "val":0.01975716537568162 - }, - "supply_limit":{ - "exp":"60 A", - "val":60.0 - }, - "stator_limit":{ - "exp":"120 A", - "val":120.0 - } - } - }, - "generationFeatures":[], - "codegen":{ - "root":null, - "genVars":true, - "genTrajData":true, - "useChoreoLib":true - } -} diff --git a/test-projects/swerve-fast/project.chor b/test-projects/swerve-fast/project.chor deleted file mode 100755 index 68fbd449ad..0000000000 --- a/test-projects/swerve-fast/project.chor +++ /dev/null @@ -1,110 +0,0 @@ -{ - "name":"project", - "version":3, - "type":"Swerve", - "variables":{ - "expressions":{}, - "poses":{} - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"11 in", - "val":0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "backLeft":{ - "x":{ - "exp":"-11 in", - "val":-0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "mass":{ - "exp":"125 lbs", - "val":56.69904625 - }, - "inertia":{ - "exp":"5 kg m ^ 2", - "val":5.0 - }, - "gearing":{ - "exp":"5.2", - "val":5.2 - }, - "radius":{ - "exp":"2 in", - "val":0.0508 - }, - "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 - }, - "tmax":{ - "exp":"1.2 N * m", - "val":1.2 - }, - "cof":{ - "exp":"2.0", - "val":2.0 - }, - "bumper":{ - "front":{ - "exp":"16 in", - "val":0.4064 - }, - "side":{ - "exp":"16 in", - "val":0.4064 - }, - "back":{ - "exp":"16 in", - "val":0.4064 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - }, - "motorConfig":{ - "free_speed":{ - "exp":"5800 RPM", - "val":607.3745796940267 - }, - "stall_torque":{ - "exp":"9.36 N * m", - "val":9.36 - }, - "kT":{ - "exp":"0.02 N * m / A", - "val":0.02 - }, - "kV":{ - "exp":"0.00206896552 V / rpm", - "val":0.01975716537568162 - }, - "supply_limit":{ - "exp":"60 A", - "val":60.0 - }, - "stator_limit":{ - "exp":"120 A", - "val":120.0 - } - } - }, - "generationFeatures":[], - "codegen":{ - "root":null, - "genVars":true, - "genTrajData":true, - "useChoreoLib":true - } -} diff --git a/test-projects/swerve-heavy/project.chor b/test-projects/swerve-heavy/project.chor deleted file mode 100755 index b3277d63e0..0000000000 --- a/test-projects/swerve-heavy/project.chor +++ /dev/null @@ -1,110 +0,0 @@ -{ - "name":"project", - "version":3, - "type":"Swerve", - "variables":{ - "expressions":{}, - "poses":{} - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"11 in", - "val":0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "backLeft":{ - "x":{ - "exp":"-11 in", - "val":-0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "mass":{ - "exp":"150 lbs", - "val":68.0388555 - }, - "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 - }, - "gearing":{ - "exp":"6.5", - "val":6.5 - }, - "radius":{ - "exp":"2 in", - "val":0.0508 - }, - "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 - }, - "tmax":{ - "exp":"1.2 N * m", - "val":1.2 - }, - "cof":{ - "exp":"2.0", - "val":2.0 - }, - "bumper":{ - "front":{ - "exp":"16 in", - "val":0.4064 - }, - "side":{ - "exp":"16 in", - "val":0.4064 - }, - "back":{ - "exp":"16 in", - "val":0.4064 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - }, - "motorConfig":{ - "free_speed":{ - "exp":"5800 RPM", - "val":607.3745796940267 - }, - "stall_torque":{ - "exp":"9.36 N * m", - "val":9.36 - }, - "kT":{ - "exp":"0.02 N * m / A", - "val":0.02 - }, - "kV":{ - "exp":"0.00206896552 V / rpm", - "val":0.01975716537568162 - }, - "supply_limit":{ - "exp":"60 A", - "val":60.0 - }, - "stator_limit":{ - "exp":"120 A", - "val":120.0 - } - } - }, - "generationFeatures":[], - "codegen":{ - "root":null, - "genVars":true, - "genTrajData":true, - "useChoreoLib":true - } -} diff --git a/test-projects/swerve-weak/project.chor b/test-projects/swerve-weak/project.chor deleted file mode 100755 index e16c369e91..0000000000 --- a/test-projects/swerve-weak/project.chor +++ /dev/null @@ -1,110 +0,0 @@ -{ - "name":"project", - "version":3, - "type":"Swerve", - "variables":{ - "expressions":{}, - "poses":{} - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"11 in", - "val":0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "backLeft":{ - "x":{ - "exp":"-11 in", - "val":-0.2794 - }, - "y":{ - "exp":"11 in", - "val":0.2794 - } - }, - "mass":{ - "exp":"125 lbs", - "val":56.69904625 - }, - "inertia":{ - "exp":"5 kg m ^ 2", - "val":5.0 - }, - "gearing":{ - "exp":"6.5", - "val":6.5 - }, - "radius":{ - "exp":"2 in", - "val":0.0508 - }, - "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 - }, - "tmax":{ - "exp":"1.2 N * m", - "val":1.2 - }, - "cof":{ - "exp":"2.0", - "val":2.0 - }, - "bumper":{ - "front":{ - "exp":"16 in", - "val":0.4064 - }, - "side":{ - "exp":"16 in", - "val":0.4064 - }, - "back":{ - "exp":"16 in", - "val":0.4064 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - }, - "motorConfig":{ - "free_speed":{ - "exp":"5800 RPM", - "val":607.3745796940267 - }, - "stall_torque":{ - "exp":"4.20 N * m", - "val":4.20 - }, - "kT":{ - "exp":"0.02 N * m / A", - "val":0.02 - }, - "kV":{ - "exp":"0.01975716537568162 V / rpm", - "val":0.01975716537568162 - }, - "supply_limit":{ - "exp":"50 A", - "val":50.0 - }, - "stator_limit":{ - "exp":"50 A", - "val":50.0 - } - } - }, - "generationFeatures":[], - "codegen":{ - "root":null, - "genVars":true, - "genTrajData":true, - "useChoreoLib":true - } -} From 937a5b4ebb3c28993aa0c48964f99ef323120bcd Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 06:19:51 -0400 Subject: [PATCH 22/53] CI bench header improvement --- scripts/bench-report.mjs | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/scripts/bench-report.mjs b/scripts/bench-report.mjs index c74e16a36e..1a35c376bb 100644 --- a/scripts/bench-report.mjs +++ b/scripts/bench-report.mjs @@ -231,13 +231,20 @@ function renderMarkdown(rows, summary, artifactUrl, commit, rendersUrl) { } const cmp = rs.filter(r => r.status === "OK" || r.status === "FLAKY"); + // Denominator is the variant's trajectory count (rs.length), never a + // hardcoded 4. comparable = generated on BOTH sides; PR / base = generated + // on that side regardless of the other, so a one-sided failure is obvious. + const total = rs.length; + const prOkN = rs.filter(r => r.prOk).length; + const baseOkN = rs.filter(r => r.baseOk).length; + const counts = `${cmp.length}/${total} comparable · ${prOkN}/${total} PR · ${baseOkN}/${total} base`; let verdict; if (cmp.length > 0) { const sp = cmp.reduce((a, r) => a + r.pr.mean, 0); const sb = cmp.reduce((a, r) => a + r.base.mean, 0); - verdict = `solve ${signedPct(sb > 0 ? ((sp - sb) / sb) * 100 : 0)} (${cmp.length}/${rs.length} comparable)`; + verdict = `solve ${signedPct(sb > 0 ? ((sp - sb) / sb) * 100 : 0)} (${counts})`; } else { - verdict = `0/${rs.length} comparable`; + verdict = counts; } const open = variant === worstVariant ? " open" : ""; L.push(`${variant} — ${verdict}`); From 4207b8c3d72bf1bd25f518e5c693060be71de222 Mon Sep 17 00:00:00 2001 From: Ishaan Agrawal Date: Sat, 16 May 2026 16:38:30 -0400 Subject: [PATCH 23/53] rework CI bench parallelism --- scripts/bench-local.sh | 58 ++++++++++++++++--- scripts/bench-report.mjs | 4 +- scripts/run-bench.sh | 120 +++++++++++++++++++++++++++++++++++---- 3 files changed, 162 insertions(+), 20 deletions(-) diff --git a/scripts/bench-local.sh b/scripts/bench-local.sh index e4aa71a8b3..2f59eb6891 100755 --- a/scripts/bench-local.sh +++ b/scripts/bench-local.sh @@ -7,9 +7,19 @@ # scripts/bench-local.sh # PR-only run, no comparison # scripts/bench-local.sh # full comparison vs (e.g. main) # -# Requires: cargo, node, and `timeout` (GNU coreutils — run-bench.sh caps each -# run at 20m). The base build uses `git worktree` so your current checkout stays -# untouched. +# Requires: cargo and node (node also performs run-bench.sh's per-trajectory +# JSON merge). The base build uses `git worktree` so your current checkout +# stays untouched. +# +# Concurrency (locally we want to saturate the box, unlike CI's 1-variant-per- +# job matrix): variants are benched in parallel, and within each variant +# run-bench.sh runs that variant's trajectories in parallel too. Peak solver +# processes ≈ BENCH_VARIANT_JOBS × BENCH_TRAJ_JOBS, so size the product to your +# core count: +# BENCH_VARIANT_JOBS max variants in parallel (default: nproc/4, min 1) +# BENCH_TRAJ_JOBS trajectories per variant (run-bench.sh, default 4) +# Defaults give ≈ nproc concurrent solves. Example: +# BENCH_VARIANT_JOBS=8 BENCH_TRAJ_JOBS=4 scripts/bench-local.sh set -euo pipefail @@ -17,6 +27,8 @@ REPO_ROOT=$(git -C "$(dirname "$0")" rev-parse --show-toplevel) cd "$REPO_ROOT" BASE_REF=${1:-} +NPROC=$(nproc 2>/dev/null || echo 4) +VARIANT_JOBS=${BENCH_VARIANT_JOBS:-$(( NPROC / 4 > 0 ? NPROC / 4 : 1 ))} OUT_ROOT="$REPO_ROOT/bench-local" PR_TP="$OUT_ROOT/tp-pr" BASE_TP="$OUT_ROOT/tp-base" @@ -28,10 +40,42 @@ if [ ! -d "$REPO_ROOT/test-projects" ]; then echo "test-projects/ not found at repo root" >&2 exit 1 fi -for cmd in cargo node timeout; do +for cmd in cargo node; do command -v "$cmd" >/dev/null || { echo "missing required command: $cmd" >&2; exit 1; } done +# Bench every variant under $2 with $1, writing reports to $3, running up to +# $VARIANT_JOBS variants concurrently. Each variant is a separate run-bench.sh +# process (its own SHARD_ROOT, its own variant-prefixed report files — no +# cross-variant collision in the shared out dir). Per-variant output is teed to +# bench-local/logs/