From 6988ff38fadf9e683dc335369ac41bd79dd2ca87 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 23 Feb 2024 09:32:07 -0800 Subject: [PATCH] Replace Sleipnir with Jormungandr --- .pylintrc | 1 + Makefile | 22 +--- motion-planning/CMakeLists.txt | 26 ----- .../DoubleIntegratorMinimumTime.cpp | 84 ------------- .../double_integrator_minimum_time.py | 110 ++++++++++++++++++ motion-planning/trajectory-optimization.tex | 49 +++++--- setup_archlinux.sh | 3 +- setup_macos.sh | 3 +- setup_ubuntu.sh | 3 +- snippets/sleipnir_csv_to_pdf.py | 42 ------- 10 files changed, 145 insertions(+), 198 deletions(-) delete mode 100644 motion-planning/CMakeLists.txt delete mode 100644 motion-planning/DoubleIntegratorMinimumTime.cpp create mode 100755 motion-planning/double_integrator_minimum_time.py delete mode 100755 snippets/sleipnir_csv_to_pdf.py diff --git a/.pylintrc b/.pylintrc index 0b5b993..dfbce2c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -423,6 +423,7 @@ disable=bad-inline-option, locally-disabled, no-else-return, no-member, + no-name-in-module, raw-checker-failed, redefined-outer-name, suppressed-message, diff --git a/Makefile b/Makefile index b49c8c4..e7a2848 100644 --- a/Makefile +++ b/Makefile @@ -91,27 +91,9 @@ build/venv.stamp: @mkdir -p $(@D) python3 setup_venv.py $(VENV_PIP) install -e ./bookutil - $(VENV_PIP) install frccontrol==2023.28 pylint qrcode requests robotpy-wpimath==2024.0.0b3.post1 + $(VENV_PIP) install frccontrol==2023.28 sleipnirgroup-jormungandr==0.0.1.dev23 pylint qrcode requests robotpy-wpimath==2024.0.0b3.post1 @touch $@ -ifeq ($(OS),Windows_NT) -$(CPP_EXE): build/%.exe: %.cpp build/venv.stamp -else -$(CPP_EXE): build/%: %.cpp build/venv.stamp -endif - @mkdir -p $(@D) - # Run CMake - cmake -B $(@D) -S $(dir $<) - # Build and run binary - cmake --build $(@D) --target $(notdir $(basename $@)) -ifeq ($(OS),Windows_NT) - cd $(@D)/Debug && ./$(notdir $@) -else - cd $(@D) && ./$(notdir $@) -endif - # Convert generated CSVs to PDFs - $(abspath $(VENV_PYTHON)) ./snippets/sleipnir_csv_to_pdf.py $(@D)/*.csv - $(PY_STAMP): build/%.stamp: %.py build/venv.stamp @mkdir -p $(@D) cd $(@D) && $(abspath $(VENV_PYTHON)) $(abspath ./$<) --noninteractive @@ -129,8 +111,6 @@ format: # Format .py find . -type f -name '*.py' -not -path './build/*' -print0 | xargs -0 python3 -m autoflake -i find . -type f -name '*.py' -not -path './build/*' -print0 | xargs -0 python3 -m black -q - # Format .cmake - find . -type f \( -name CMakeLists.txt -o -name '*.cmake' \) -not -path './build/*' -print0 | xargs -0 python3 -m gersemi -i -q # Run formatters and all .tex linters. The commit metadata files and files # generated by Python scripts are dependencies because check_tex_includes.py diff --git a/motion-planning/CMakeLists.txt b/motion-planning/CMakeLists.txt deleted file mode 100644 index 9d39453..0000000 --- a/motion-planning/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.21) - -project(DoubleIntegratorMinimumTime) - -file(GLOB src ./*.cpp) -get_filename_component(exec ${src} NAME_WE) - -include(FetchContent) - -fetchcontent_declare( - Sleipnir - GIT_REPOSITORY https://github.com/SleipnirGroup/Sleipnir.git - # main on 2024-02-23 - GIT_TAG e4fdf64633b62416e07da8c10ead4acba46673c4 -) -fetchcontent_makeavailable(Sleipnir) - -fetchcontent_declare( - fmt - GIT_REPOSITORY https://github.com/fmtlib/fmt.git - GIT_TAG 9.1.0 -) -fetchcontent_makeavailable(fmt) - -add_executable(${exec} ${src}) -target_link_libraries(${exec} PRIVATE Sleipnir fmt) diff --git a/motion-planning/DoubleIntegratorMinimumTime.cpp b/motion-planning/DoubleIntegratorMinimumTime.cpp deleted file mode 100644 index 3cf84e2..0000000 --- a/motion-planning/DoubleIntegratorMinimumTime.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include -#include - -#include -#include -#include - -int main() { - using namespace std::chrono_literals; - - constexpr auto T = 3.5s; - constexpr auto dt = 5ms; - constexpr int N = T / dt; - - constexpr double r = 2.0; - - sleipnir::OptimizationProblem problem; - - // 2x1 state vector with N + 1 timesteps (includes last state) - auto X = problem.DecisionVariable(2, N + 1); - - // 1x1 input vector with N timesteps (input at last state doesn't matter) - auto U = problem.DecisionVariable(1, N); - - // Kinematics constraint assuming constant acceleration between timesteps - for (int k = 0; k < N; ++k) { - constexpr double t = std::chrono::duration{dt}.count(); - auto p_k1 = X(0, k + 1); - auto v_k1 = X(1, k + 1); - auto p_k = X(0, k); - auto v_k = X(1, k); - auto a_k = U(0, k); - - problem.SubjectTo(p_k1 == p_k + v_k * t); - problem.SubjectTo(v_k1 == v_k + a_k * t); - } - - // Start and end at rest - problem.SubjectTo(X.Col(0) == Eigen::Matrix{{0.0}, {0.0}}); - problem.SubjectTo(X.Col(N) == Eigen::Matrix{{r}, {0.0}}); - - // Limit velocity - problem.SubjectTo(-1 <= X.Row(1)); - problem.SubjectTo(X.Row(1) <= 1); - - // Limit acceleration - problem.SubjectTo(-1 <= U); - problem.SubjectTo(U <= 1); - - // Cost function - minimize position error - sleipnir::Variable J = 0.0; - for (int k = 0; k < N + 1; ++k) { - J += sleipnir::pow(r - X(0, k), 2); - } - problem.Minimize(J); - - problem.Solve(); - - // Log states for offline viewing - std::ofstream states{"DoubleIntegratorMinimumTimeStates.csv"}; - if (states.is_open()) { - states << "Time (s),Position (m),Velocity (m/s)\n"; - - constexpr double t = std::chrono::duration{dt}.count(); - for (int k = 0; k < N + 1; ++k) { - states << fmt::format("{},{},{}\n", k * t, X.Value(0, k), X.Value(1, k)); - } - } - - // Log inputs for offline viewing - std::ofstream inputs{"DoubleIntegratorMinimumTimeInputs.csv"}; - if (inputs.is_open()) { - inputs << "Time (s),Acceleration (m/s²)\n"; - - constexpr double t = std::chrono::duration{dt}.count(); - for (int k = 0; k < N + 1; ++k) { - if (k < N) { - inputs << fmt::format("{},{}\n", k * t, U.Value(0, k)); - } else { - inputs << fmt::format("{},{}\n", k * t, 0.0); - } - } - } -} diff --git a/motion-planning/double_integrator_minimum_time.py b/motion-planning/double_integrator_minimum_time.py new file mode 100755 index 0000000..e3b5513 --- /dev/null +++ b/motion-planning/double_integrator_minimum_time.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +"""Solves double integrator minimum-time trajectory optimization problem with Jormungandr.""" + +from pathlib import Path +import re +import sys + +from jormungandr.optimization import OptimizationProblem +import matplotlib as mpl +import matplotlib.pyplot as plt +import numpy as np + +from bookutil import latex + +if "--noninteractive" in sys.argv: + mpl.use("svg") +plt.rc("text", usetex=True) + + +def main(): + """Entry point.""" + T = 3.5 # s + dt = 0.005 # 5 ms + N = int(T / dt) + + r = 2.0 + + problem = OptimizationProblem() + + # 2x1 state vector with N + 1 timesteps (includes last state) + X = problem.decision_variable(2, N + 1) + + # 1x1 input vector with N timesteps (input at last state doesn't matter) + U = problem.decision_variable(1, N) + + # Kinematics constraint assuming constant acceleration between timesteps + for k in range(N): + p_k1 = X[0, k + 1] + v_k1 = X[1, k + 1] + p_k = X[0, k] + v_k = X[1, k] + a_k = U[0, k] + + problem.subject_to(p_k1 == p_k + v_k * dt) + problem.subject_to(v_k1 == v_k + a_k * dt) + + # Start and end at rest + problem.subject_to(X[:, 0] == np.array([[0.0], [0.0]])) + problem.subject_to(X[:, N] == np.array([[r], [0.0]])) + + # Limit velocity + problem.subject_to(-1 <= X[1, :]) + problem.subject_to(X[1, :] <= 1) + + # Limit acceleration + problem.subject_to(-1 <= U) + problem.subject_to(U <= 1) + + # Cost function - minimize position error + J = 0.0 + for k in range(N + 1): + J += (r - X[0, k]) ** 2 + problem.minimize(J) + + problem.solve() + + ts = [k * dt for k in range(N + 1)] + + # Plot states + labels = ["Position (m)", "Velocity (m/s)"] + for i in range(X.shape[0]): + plt.figure() + ax = plt.gca() + + xs = X.value()[i, :] + ax.plot(ts, xs, label=labels[i]) + ax.set_xlabel("Time (s)") + ax.set_ylabel(labels[i]) + + unit_name = re.search(r"(\w+)(\(.*?\))?", labels[i]).group(1).lower() + figname = f"{Path(__file__).stem}_{unit_name}" + + if "--noninteractive" in sys.argv: + latex.savefig(figname) + else: + plt.show() + + # Plot inputs + labels = ["Acceleration (m/s²)"] + for i in range(U.shape[0]): + plt.figure() + ax = plt.gca() + + us = np.concatenate((U.value()[i, :], [0.0])) + ax.plot(ts, us, label=labels[i]) + ax.set_xlabel("Time (s)") + ax.set_ylabel(labels[i]) + + unit_name = re.search(r"(\w+)(\(.*?\))?", labels[i]).group(1).lower() + figname = f"{Path(__file__).stem}_{unit_name}" + + if "--noninteractive" in sys.argv: + latex.savefig(figname) + else: + plt.show() + + +if __name__ == "__main__": + main() diff --git a/motion-planning/trajectory-optimization.tex b/motion-planning/trajectory-optimization.tex index f42111c..4801350 100644 --- a/motion-planning/trajectory-optimization.tex +++ b/motion-planning/trajectory-optimization.tex @@ -48,18 +48,29 @@ \subsection{Double integrator minimum time} acceleration. The velocity constraints are $-1 \leq x(1) \leq 1$ and the acceleration constraints are $-1 \leq u \leq 1$. +\subsubsection{Importing required libraries} + +Jormungandr is the Python version of Sleipnir and can be installed via +\texttt{pip install sleipnirgroup-jormungandr}. +\begin{code} + \begin{lstlisting}[style=customPython] + from jormungandr.optimization import OptimizationProblem + import numpy as np + \end{lstlisting} +\end{code} + \subsubsection{Initializing a problem instance} First, we need to make a problem instance. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{1}{17} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{23}{29} \end{coderemotesubset} \subsubsection{Creating decision variables} Next, we need to make decision variables for our state and input. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{19}{23} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{31}{35} \end{coderemotesubset} By convention, we use capital letters for the variables to designate @@ -68,20 +79,20 @@ \subsubsection{Creating decision variables} \subsubsection{Applying constraints} Now, we need to apply dynamics constraints between timesteps. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{25}{36} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{37}{46} \end{coderemotesubset} Next, we'll apply the state and input constraints. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{38}{48} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{48}{58} \end{coderemotesubset} \subsubsection{Specifying a cost function} Next, we'll create a cost function for minimizing position error. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{50}{55} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{60}{64} \end{coderemotesubset} The cost function passed to Minimize() should produce a scalar output. @@ -89,8 +100,8 @@ \subsubsection{Specifying a cost function} \subsubsection{Solving the problem} Now we can solve the problem. -\begin{coderemotesubset}{cpp} - {motion-planning/DoubleIntegratorMinimumTime.cpp}{57}{57} +\begin{coderemotesubset}{Python} + {motion-planning/double_integrator_minimum_time.py}{66}{66} \end{coderemotesubset} The solver will find the decision variable values that minimize the cost @@ -100,22 +111,22 @@ \subsubsection{Accessing the solution} You can obtain the solution by querying the values of the variables like so. \begin{code} - \begin{lstlisting}[style=customCpp] -double position = X.Value(0, 0); -double velocity = X.Value(1, 0); -double acceleration = U.Value(0); + \begin{lstlisting}[style=customPython] + position = X.value[0, 0] + velocity = X.value[1, 0] + acceleration = U.value(0) \end{lstlisting} \end{code} \begin{bookfigure} - \begin{minisvg}{2}{build/motion-planning/DoubleIntegratorMinimumTimeStatesPosition} + \begin{minisvg}{2}{build/motion-planning/double_integrator_minimum_time_position} \caption{Double integrator position} \end{minisvg} \hfill - \begin{minisvg}{2}{build/motion-planning/DoubleIntegratorMinimumTimeStatesVelocity} + \begin{minisvg}{2}{build/motion-planning/double_integrator_minimum_time_velocity} \caption{Double integrator velocity} \end{minisvg} \hfill - \begin{minisvg}{2}{build/motion-planning/DoubleIntegratorMinimumTimeInputsAcceleration} + \begin{minisvg}{2}{build/motion-planning/double_integrator_minimum_time_acceleration} \caption{Double integrator acceleration} \end{minisvg} \end{bookfigure} diff --git a/setup_archlinux.sh b/setup_archlinux.sh index 09924bf..bab973c 100755 --- a/setup_archlinux.sh +++ b/setup_archlinux.sh @@ -48,5 +48,4 @@ sudo pacman -Sy --needed --noconfirm \ texlive-xetex # autoflake isn't in [extra] and we can't use an AUR helper -# gersemi isn't in [extra] or the AUR -pip3 install --user --break-system-packages autoflake gersemi +pip3 install --user --break-system-packages autoflake diff --git a/setup_macos.sh b/setup_macos.sh index aa9c38c..1b03daa 100755 --- a/setup_macos.sh +++ b/setup_macos.sh @@ -64,6 +64,5 @@ sudo /Library/TeX/texbin/tlmgr install \ # Python packages # * black (to format Python source code) -# * gersemi (to format CMake files) # * pylint (for Python linting) -pip3 install --user autoflake black gersemi pylint qrcode requests wheel +pip3 install --user autoflake black pylint qrcode requests wheel diff --git a/setup_ubuntu.sh b/setup_ubuntu.sh index e1c5704..36cef38 100755 --- a/setup_ubuntu.sh +++ b/setup_ubuntu.sh @@ -45,6 +45,5 @@ sudo apt-get install -y \ # The Ubuntu 22.04 packages are too old # Python packages # * black (to format Python source code) -# * gersemi (to format CMake files) # * pylint (for Python linting) -pip3 install --user autoflake black gersemi pylint qrcode +pip3 install --user autoflake black pylint qrcode diff --git a/snippets/sleipnir_csv_to_pdf.py b/snippets/sleipnir_csv_to_pdf.py deleted file mode 100755 index 07b22a0..0000000 --- a/snippets/sleipnir_csv_to_pdf.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python3 - -"""Generates plot PDFs from Sleipnir CSV files.""" - -import re -import sys - -import matplotlib as mpl -import matplotlib.pyplot as plt -import numpy as np - -from bookutil import latex - -if "--noninteractive" in sys.argv: - mpl.use("svg") -plt.rc("text", usetex=True) - - -def main(): - """Entry point.""" - filenames = sys.argv[1:] - - for filename in filenames: - with open(filename, encoding="utf-8") as f: - labels = f.readline().rstrip().split(",") - data = np.genfromtxt(filename, delimiter=",", skip_header=1) - - for i in range(data.shape[1] - 1): - label = labels[i + 1] - - plt.figure() - ax = plt.gca() - ax.plot(data[:, :1], data[:, i + 1 : i + 2], label=label) - ax.set_xlabel(labels[0]) - ax.set_ylabel(label) - - figname = re.search(r"(\w+)(\(.*?\))?", label).group(1) - latex.savefig(f"{filename.removesuffix('.csv')}{figname}") - - -if __name__ == "__main__": - main()