diff --git a/jormungandr/test/optimization/optimization_problem_cart_pole_test.py b/jormungandr/test/optimization/optimization_problem_cart_pole_test.py index 46472cac..1f895abb 100644 --- a/jormungandr/test/optimization/optimization_problem_cart_pole_test.py +++ b/jormungandr/test/optimization/optimization_problem_cart_pole_test.py @@ -1,7 +1,7 @@ import math import jormungandr.autodiff as autodiff -from jormungandr.autodiff import ExpressionType, VariableMatrix +from jormungandr.autodiff import ExpressionType, VariableMatrix, Variable from jormungandr.optimization import OptimizationProblem, SolverExitCondition import numpy as np import pytest @@ -18,35 +18,37 @@ def rk4(f, x, u, dt): return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) -def cart_pole_dynamics_double(x, u): - # https://underactuated.mit.edu/acrobot.html#cart_pole - # - # θ is CCW+ measured from negative y-axis. - # - # q = [x, θ]ᵀ - # q̇ = [ẋ, θ̇]ᵀ - # u = f_x - # - # M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu - # M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu - # q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) - # - # [ m_c + m_p m_p l cosθ] - # M(q) = [m_p l cosθ m_p l² ] - # - # [0 −m_p lθ̇ sinθ] - # C(q, q̇) = [0 0 ] - # - # [ 0 ] - # τ_g(q) = [-m_p gl sinθ] - # - # [1] - # B = [0] - m_c = 5.0 # Cart mass (kg) - m_p = 0.5 # Pole mass (kg) - l = 0.5 # Pole length (m) - g = 9.806 # Acceleration due to gravity (m/s²) +# https://underactuated.mit.edu/acrobot.html#cart_pole +# +# θ is CCW+ measured from negative y-axis. +# +# q = [x, θ]ᵀ +# q̇ = [ẋ, θ̇]ᵀ +# u = f_x +# +# M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu +# M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu +# q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) +# +# [ m_c + m_p m_p l cosθ] +# M(q) = [m_p l cosθ m_p l² ] +# +# [0 −m_p lθ̇ sinθ] +# C(q, q̇) = [0 0 ] +# +# [ 0 ] +# τ_g(q) = [-m_p gl sinθ] +# +# [1] +# B = [0] + +m_c = 5.0 # Cart mass (kg) +m_p = 0.5 # Pole mass (kg) +l = 0.5 # Pole length (m) +g = 9.806 # Acceleration due to gravity (m/s²) + +def cart_pole_dynamics_double(x, u): q = x[:2, :] qdot = x[2:, :] theta = q[1, 0] @@ -54,33 +56,25 @@ def cart_pole_dynamics_double(x, u): # [ m_c + m_p m_p l cosθ] # M(q) = [m_p l cosθ m_p l² ] - M = np.empty((2, 2)) - M[0, 0] = m_c + m_p - M[0, 1] = m_p * l * math.cos(theta) - M[1, 0] = m_p * l * math.cos(theta) - M[1, 1] = m_p * l**2 - - Minv = np.empty((2, 2)) - Minv[0, 0] = M[1, 1] - Minv[0, 1] = -M[0, 1] - Minv[1, 0] = -M[1, 0] - Minv[1, 1] = M[0, 0] + M = np.array( + [ + [m_c + m_p, m_p * l * math.cos(theta)], + [m_p * l * math.cos(theta), m_p * l**2], + ] + ) + detM = M[0, 0] * M[1, 1] - M[0, 1] * M[1, 0] - Minv /= detM + Minv = np.array( + [[M[1, 1] / detM, -M[0, 1] / detM], [-M[1, 0] / detM, M[0, 0] / detM]] + ) # [0 −m_p lθ̇ sinθ] # C(q, q̇) = [0 0 ] - C = np.empty((2, 2)) - C[0, 0] = 0.0 - C[0, 1] = -m_p * l * thetadot * math.sin(theta) - C[1, 0] = 0.0 - C[1, 1] = 0.0 + C = np.array([[0.0, -m_p * l * thetadot * math.sin(theta)], [0.0, 0.0]]) # [ 0 ] # τ_g(q) = [-m_p gl sinθ] - tau_g = np.empty((2, 1)) - tau_g[0, :] = 0.0 - tau_g[1, :] = -m_p * g * l * math.sin(theta) + tau_g = np.array([[0.0], [-m_p * g * l * math.sin(theta)]]) # [1] # B = [0] @@ -94,34 +88,6 @@ def cart_pole_dynamics_double(x, u): def cart_pole_dynamics(x, u): - # https://underactuated.mit.edu/acrobot.html#cart_pole - # - # θ is CCW+ measured from negative y-axis. - # - # q = [x, θ]ᵀ - # q̇ = [ẋ, θ̇]ᵀ - # u = f_x - # - # M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu - # M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu - # q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) - # - # [ m_c + m_p m_p l cosθ] - # M(q) = [m_p l cosθ m_p l² ] - # - # [0 −m_p lθ̇ sinθ] - # C(q, q̇) = [0 0 ] - # - # [ 0 ] - # τ_g(q) = [-m_p gl sinθ] - # - # [1] - # B = [0] - m_c = 5.0 # Cart mass (kg) - m_p = 0.5 # Pole mass (kg) - l = 0.5 # Pole length (m) - g = 9.806 # Acceleration due to gravity (m/s²) - q = x[:2, :] qdot = x[2:, :] theta = q[1, 0] @@ -129,33 +95,30 @@ def cart_pole_dynamics(x, u): # [ m_c + m_p m_p l cosθ] # M(q) = [m_p l cosθ m_p l² ] - M = VariableMatrix(2, 2) - M[0, 0] = m_c + m_p - M[0, 1] = m_p * l * autodiff.cos(theta) - M[1, 0] = m_p * l * autodiff.cos(theta) - M[1, 1] = m_p * l**2 - - Minv = VariableMatrix(2, 2) - Minv[0, 0] = M[1, 1] - Minv[0, 1] = -M[0, 1] - Minv[1, 0] = -M[1, 0] - Minv[1, 1] = M[0, 0] + M = VariableMatrix( + [ + [Variable(m_c + m_p), m_p * l * autodiff.cos(theta)], + [m_p * l * autodiff.cos(theta), Variable(m_p * l**2)], + ] + ) + detM = M[0, 0] * M[1, 1] - M[0, 1] * M[1, 0] - Minv /= detM + Minv = VariableMatrix( + [[M[1, 1] / detM, -M[0, 1] / detM], [-M[1, 0] / detM, M[0, 0] / detM]] + ) # [0 −m_p lθ̇ sinθ] # C(q, q̇) = [0 0 ] - C = VariableMatrix(2, 2) - C[0, 0] = 0.0 - C[0, 1] = -m_p * l * thetadot * autodiff.sin(theta) - C[1, 0] = 0.0 - C[1, 1] = 0.0 + C = VariableMatrix( + [ + [Variable(0.0), -m_p * l * thetadot * autodiff.sin(theta)], + [Variable(0.0), Variable(0.0)], + ] + ) # [ 0 ] # τ_g(q) = [-m_p gl sinθ] - tau_g = VariableMatrix(2, 1) - tau_g[0, :] = 0.0 - tau_g[1, :] = -m_p * g * l * autodiff.sin(theta) + tau_g = VariableMatrix([[Variable(0.0)], [-m_p * g * l * autodiff.sin(theta)]]) # [1] # B = [0] diff --git a/jormungandr/test/optimization/optimization_problem_differential_drive_test.py b/jormungandr/test/optimization/optimization_problem_differential_drive_test.py index 92f25459..54e7eaca 100644 --- a/jormungandr/test/optimization/optimization_problem_differential_drive_test.py +++ b/jormungandr/test/optimization/optimization_problem_differential_drive_test.py @@ -21,55 +21,43 @@ def rk4(f, x, u, dt): return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) -def differential_drive_dynamics_double(x, u): - # x = [x, y, heading, left velocity, right velocity]ᵀ - # u = [left voltage, right voltage]ᵀ - trackwidth = 0.699 # m - Kv_linear = 3.02 # V/(m/s) - Ka_linear = 0.642 # V/(m/s²) - Kv_angular = 1.382 # V/(m/s) - Ka_angular = 0.08495 # V/(m/s²) - - v = (x[3, 0] + x[4, 0]) / 2.0 +# x = [x, y, heading, left velocity, right velocity]ᵀ +# u = [left voltage, right voltage]ᵀ +trackwidth = 0.699 # m +Kv_linear = 3.02 # V/(m/s) +Ka_linear = 0.642 # V/(m/s²) +Kv_angular = 1.382 # V/(m/s) +Ka_angular = 0.08495 # V/(m/s²) + +A1 = -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0 +A2 = -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0 +B1 = 0.5 / Ka_linear + 0.5 / Ka_angular +B2 = 0.5 / Ka_linear - 0.5 / Ka_angular +A = np.array([[A1, A2], [A2, A1]]) +B = np.array([[B1, B2], [B2, B1]]) - A1 = -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0 - A2 = -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0 - B1 = 0.5 / Ka_linear + 0.5 / Ka_angular - B2 = 0.5 / Ka_linear - 0.5 / Ka_angular - A = np.array([[A1, A2], [A2, A1]]) - B = np.array([[B1, B2], [B2, B1]]) +def differential_drive_dynamics_double(x, u): xdot = np.empty((5, 1)) + + v = (x[3, 0] + x[4, 0]) / 2.0 xdot[0, :] = v * math.cos(x[2, 0]) xdot[1, :] = v * math.sin(x[2, 0]) xdot[2, :] = (x[4, 0] - x[3, 0]) / trackwidth xdot[3:5, :] = A @ x[3:5, :] + B @ u + return xdot def differential_drive_dynamics(x, u): - # x = [x, y, heading, left velocity, right velocity]ᵀ - # u = [left voltage, right voltage]ᵀ - trackwidth = 0.699 # m - Kv_linear = 3.02 # V/(m/s) - Ka_linear = 0.642 # V/(m/s²) - Kv_angular = 1.382 # V/(m/s) - Ka_angular = 0.08495 # V/(m/s²) + xdot = VariableMatrix(5, 1) v = (x[3, 0] + x[4, 0]) / 2.0 - - A1 = -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0 - A2 = -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0 - B1 = 0.5 / Ka_linear + 0.5 / Ka_angular - B2 = 0.5 / Ka_linear - 0.5 / Ka_angular - A = np.array([[A1, A2], [A2, A1]]) - B = np.array([[B1, B2], [B2, B1]]) - - xdot = VariableMatrix(5, 1) xdot[0, :] = v * autodiff.cos(x[2, 0]) xdot[1, :] = v * autodiff.sin(x[2, 0]) xdot[2, :] = (x[4, 0] - x[3, 0]) / trackwidth xdot[3:5, :] = A @ x[3:5, :] + B @ u + return xdot diff --git a/test/src/CartPoleUtil.cpp b/test/src/CartPoleUtil.cpp index f79bf394..22ab9f00 100644 --- a/test/src/CartPoleUtil.cpp +++ b/test/src/CartPoleUtil.cpp @@ -2,74 +2,65 @@ #include "CartPoleUtil.hpp" +// https://underactuated.mit.edu/acrobot.html#cart_pole +// +// θ is CCW+ measured from negative y-axis. +// +// q = [x, θ]ᵀ +// q̇ = [ẋ, θ̇]ᵀ +// u = f_x +// +// M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu +// M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu +// q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) +// +// [ m_c + m_p m_p l cosθ] +// M(q) = [m_p l cosθ m_p l² ] +// +// [0 −m_p lθ̇ sinθ] +// C(q, q̇) = [0 0 ] +// +// [ 0 ] +// τ_g(q) = [-m_p gl sinθ] +// +// [1] +// B = [0] + +inline constexpr double m_c = 5.0; // Cart mass (kg) +inline constexpr double m_p = 0.5; // Pole mass (kg) +inline constexpr double l = 0.5; // Pole length (m) +inline constexpr double g = 9.806; // Acceleration due to gravity (m/s²) + Eigen::Vector CartPoleDynamicsDouble( const Eigen::Vector& x, const Eigen::Vector& u) { - // https://underactuated.mit.edu/acrobot.html#cart_pole - // - // θ is CCW+ measured from negative y-axis. - // - // q = [x, θ]ᵀ - // q̇ = [ẋ, θ̇]ᵀ - // u = f_x - // - // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu - // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu - // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) - // - // [ m_c + m_p m_p l cosθ] - // M(q) = [m_p l cosθ m_p l² ] - // - // [0 −m_p lθ̇ sinθ] - // C(q, q̇) = [0 0 ] - // - // [ 0 ] - // τ_g(q) = [-m_p gl sinθ] - // - // [1] - // B = [0] - constexpr double m_c = 5.0; // Cart mass (kg) - constexpr double m_p = 0.5; // Pole mass (kg) - constexpr double l = 0.5; // Pole length (m) - constexpr double g = 9.806; // Acceleration due to gravity (m/s²) - - Eigen::Vector q = x.segment(0, 2); - Eigen::Vector qdot = x.segment(2, 2); - double theta = q(1); - double thetadot = qdot(1); + auto q = x.segment(0, 2); + auto qdot = x.segment(2, 2); + auto theta = q(1); + auto thetadot = qdot(1); // [ m_c + m_p m_p l cosθ] // M(q) = [m_p l cosθ m_p l² ] - Eigen::Matrix M; - M(0, 0) = m_c + m_p; - M(0, 1) = m_p * l * cos(theta); // NOLINT - M(1, 0) = m_p * l * cos(theta); // NOLINT - M(1, 1) = m_p * std::pow(l, 2); - - Eigen::Matrix Minv; - Minv(0, 0) = M(1, 1); - Minv(0, 1) = -M(0, 1); - Minv(1, 0) = -M(1, 0); - Minv(1, 1) = M(0, 0); + Eigen::Matrix M{ + {m_c + m_p, m_p * l * cos(theta)}, // NOLINT + {m_p * l * cos(theta), m_p * std::pow(l, 2)}}; // NOLINT + double detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0); - Minv /= detM; + Eigen::Matrix Minv{{M(1, 1) / detM, -M(0, 1) / detM}, + {-M(1, 0) / detM, M(0, 0) / detM}}; // [0 −m_p lθ̇ sinθ] // C(q, q̇) = [0 0 ] - Eigen::Matrix C; - C(0, 0) = 0; - C(0, 1) = -m_p * l * thetadot * sin(theta); // NOLINT - C(1, 0) = 0; - C(1, 1) = 0; + Eigen::Matrix C{ + {0, -m_p * l * thetadot * sin(theta)}, // NOLINT + {0, 0}}; // [ 0 ] // τ_g(q) = [-m_p gl sinθ] - Eigen::Vector tau_g; - tau_g(0) = 0; - tau_g(1) = -m_p * g * l * sin(theta); // NOLINT + Eigen::Vector tau_g{{0}, {-m_p * g * l * sin(theta)}}; // NOLINT // [1] // B = [0] - Eigen::Matrix B{{1}, {0}}; + constexpr Eigen::Matrix B{{1}, {0}}; // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) Eigen::Vector qddot; @@ -80,34 +71,6 @@ Eigen::Vector CartPoleDynamicsDouble( sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x, const sleipnir::VariableMatrix& u) { - // https://underactuated.mit.edu/acrobot.html#cart_pole - // - // θ is CCW+ measured from negative y-axis. - // - // q = [x, θ]ᵀ - // q̇ = [ẋ, θ̇]ᵀ - // u = f_x - // - // M(q)q̈ + C(q, q̇)q̇ = τ_g(q) + Bu - // M(q)q̈ = τ_g(q) − C(q, q̇)q̇ + Bu - // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) - // - // [ m_c + m_p m_p l cosθ] - // M(q) = [m_p l cosθ m_p l² ] - // - // [0 −m_p lθ̇ sinθ] - // C(q, q̇) = [0 0 ] - // - // [ 0 ] - // τ_g(q) = [-m_p gl sinθ] - // - // [1] - // B = [0] - constexpr double m_c = 5.0; // Cart mass (kg) - constexpr double m_p = 0.5; // Pole mass (kg) - constexpr double l = 0.5; // Pole length (m) - constexpr double g = 9.806; // Acceleration due to gravity (m/s²) - auto q = x.Segment(0, 2); auto qdot = x.Segment(2, 2); auto theta = q(1); @@ -115,37 +78,26 @@ sleipnir::VariableMatrix CartPoleDynamics(const sleipnir::VariableMatrix& x, // [ m_c + m_p m_p l cosθ] // M(q) = [m_p l cosθ m_p l² ] - sleipnir::VariableMatrix M{2, 2}; - M(0, 0) = m_c + m_p; - M(0, 1) = m_p * l * cos(theta); // NOLINT - M(1, 0) = m_p * l * cos(theta); // NOLINT - M(1, 1) = m_p * std::pow(l, 2); - - sleipnir::VariableMatrix Minv{2, 2}; - Minv(0, 0) = M(1, 1); - Minv(0, 1) = -M(0, 1); - Minv(1, 0) = -M(1, 0); - Minv(1, 1) = M(0, 0); + sleipnir::VariableMatrix M{ + {m_c + m_p, m_p * l * cos(theta)}, // NOLINT + {m_p * l * cos(theta), m_p * std::pow(l, 2)}}; // NOLINT + auto detM = M(0, 0) * M(1, 1) - M(0, 1) * M(1, 0); - Minv /= detM; + sleipnir::VariableMatrix Minv{{M(1, 1) / detM, -M(0, 1) / detM}, + {-M(1, 0) / detM, M(0, 0) / detM}}; // [0 −m_p lθ̇ sinθ] // C(q, q̇) = [0 0 ] - sleipnir::VariableMatrix C{2, 2}; - C(0, 0) = 0; - C(0, 1) = -m_p * l * thetadot * sin(theta); // NOLINT - C(1, 0) = 0; - C(1, 1) = 0; + sleipnir::VariableMatrix C{{0, -m_p * l * thetadot * sin(theta)}, // NOLINT + {0, 0}}; // [ 0 ] // τ_g(q) = [-m_p gl sinθ] - sleipnir::VariableMatrix tau_g{2}; - tau_g(0) = 0; - tau_g(1) = -m_p * g * l * sin(theta); // NOLINT + sleipnir::VariableMatrix tau_g{{0}, {-m_p * g * l * sin(theta)}}; // NOLINT // [1] // B = [0] - Eigen::Matrix B{{1}, {0}}; + constexpr Eigen::Matrix B{{1}, {0}}; // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu) sleipnir::VariableMatrix qddot{4}; diff --git a/test/src/DifferentialDriveUtil.cpp b/test/src/DifferentialDriveUtil.cpp index b6d6f26f..32396ea2 100644 --- a/test/src/DifferentialDriveUtil.cpp +++ b/test/src/DifferentialDriveUtil.cpp @@ -2,60 +2,46 @@ #include "DifferentialDriveUtil.hpp" +// x = [x, y, heading, left velocity, right velocity]ᵀ +// u = [left voltage, right voltage]ᵀ + +inline constexpr double trackwidth = 0.699; // m +inline constexpr double Kv_linear = 3.02; // V/(m/s) +inline constexpr double Ka_linear = 0.642; // V/(m/s²) +inline constexpr double Kv_angular = 1.382; // V/(m/s) +inline constexpr double Ka_angular = 0.08495; // V/(m/s²) + +inline constexpr double A1 = + -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0; +inline constexpr double A2 = + -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0; +inline constexpr double B1 = 0.5 / Ka_linear + 0.5 / Ka_angular; +inline constexpr double B2 = 0.5 / Ka_linear - 0.5 / Ka_angular; +inline constexpr Eigen::Matrix A{{A1, A2}, {A2, A1}}; +inline constexpr Eigen::Matrix B{{B1, B2}, {B2, B1}}; + Eigen::Vector DifferentialDriveDynamicsDouble( const Eigen::Vector& x, const Eigen::Vector& u) { - // x = [x, y, heading, left velocity, right velocity]ᵀ - // u = [left voltage, right voltage]ᵀ - constexpr double trackwidth = 0.699; // m - constexpr double Kv_linear = 3.02; // V/(m/s) - constexpr double Ka_linear = 0.642; // V/(m/s²) - constexpr double Kv_angular = 1.382; // V/(m/s) - constexpr double Ka_angular = 0.08495; // V/(m/s²) - - double v = (x(3) + x(4)) / 2.0; - - constexpr double A1 = - -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0; - constexpr double A2 = - -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0; - constexpr double B1 = 0.5 / Ka_linear + 0.5 / Ka_angular; - constexpr double B2 = 0.5 / Ka_linear - 0.5 / Ka_angular; - Eigen::Matrix A{{A1, A2}, {A2, A1}}; - Eigen::Matrix B{{B1, B2}, {B2, B1}}; - Eigen::Vector xdot; - xdot(0) = v * std::cos(x(2)); - xdot(1) = v * std::sin(x(2)); + + auto v = (x(3) + x(4)) / 2.0; + xdot(0) = v * cos(x(2)); // NOLINT + xdot(1) = v * sin(x(2)); // NOLINT xdot(2) = (x(4) - x(3)) / trackwidth; - xdot.segment(3, 2) = A * x.block<2, 1>(3, 0) + B * u; + xdot.segment(3, 2) = A * x.segment(3, 2) + B * u; + return xdot; } sleipnir::VariableMatrix DifferentialDriveDynamics( const sleipnir::VariableMatrix& x, const sleipnir::VariableMatrix& u) { - // x = [x, y, heading, left velocity, right velocity]ᵀ - // u = [left voltage, right voltage]ᵀ - constexpr double trackwidth = 0.699; // m - constexpr double Kv_linear = 3.02; // V/(m/s) - constexpr double Ka_linear = 0.642; // V/(m/s²) - constexpr double Kv_angular = 1.382; // V/(m/s) - constexpr double Ka_angular = 0.08495; // V/(m/s²) + sleipnir::VariableMatrix xdot{5}; auto v = (x(3) + x(4)) / 2.0; - - constexpr double A1 = - -(Kv_linear / Ka_linear + Kv_angular / Ka_angular) / 2.0; - constexpr double A2 = - -(Kv_linear / Ka_linear - Kv_angular / Ka_angular) / 2.0; - constexpr double B1 = 0.5 / Ka_linear + 0.5 / Ka_angular; - constexpr double B2 = 0.5 / Ka_linear - 0.5 / Ka_angular; - Eigen::Matrix A{{A1, A2}, {A2, A1}}; - Eigen::Matrix B{{B1, B2}, {B2, B1}}; - - sleipnir::VariableMatrix xdot{5}; - xdot(0) = v * sleipnir::cos(x(2)); - xdot(1) = v * sleipnir::sin(x(2)); + xdot(0) = v * cos(x(2)); // NOLINT + xdot(1) = v * sin(x(2)); // NOLINT xdot(2) = (x(4) - x(3)) / trackwidth; xdot.Segment(3, 2) = A * x.Segment(3, 2) + B * u; + return xdot; }