Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[copybara] Resync robotpy (#8585)

Browse Source 
Project import generated by Copybara.
    
GitOrigin-RevId: fd000778e9b78c72cc7ca7b2ebe476129b78c6e0
This commit is contained in:
PJ Reiniger
2026-02-08 10:36:35 -05:00
committed by GitHub
parent 4aa21e947d
commit 227c89ab23
53 changed files with 1049 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpimath/robotpy_pybind_build_info.bzl generated
View File

@@ -948,6 +948,22 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
("wpi::math::Models", "wpi__math__Models.hpp"),
],
),
struct(
class_name = "NumericalIntegration",
yml_file = "semiwrap/NumericalIntegration.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/NumericalIntegration.hpp",
tmpl_class_names = [],
trampolines = [],
),
struct(
class_name = "NumericalJacobian",
yml_file = "semiwrap/NumericalJacobian.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/NumericalJacobian.hpp",
tmpl_class_names = [],
trampolines = [],
),
struct(
class_name = "ExponentialProfile",
yml_file = "semiwrap/ExponentialProfile.yml",

8
wpimath/src/main/python/pyproject.toml
View File

@@ -1,7 +1,7 @@
[build-system]
build-backend = "hatchling.build"
requires = [
"semiwrap~=0.2.1",
"semiwrap~=0.2.6",
"hatch-meson~=0.1.0",
"hatch-robotpy~=0.2.1",
"hatchling",
@@ -69,8 +69,6 @@ scan_headers_ignore = [
"wpi/math/random/Normal.hpp",
"wpi/math/system/Discretization.hpp",
"wpi/math/system/NumericalIntegration.hpp",
"wpi/math/system/NumericalJacobian.hpp",
"wpi/math/proto/*",
"wpi/math/*/proto/*",
@@ -1566,8 +1564,8 @@ LinearSystem = "wpi/math/system/LinearSystem.hpp"
LinearSystemLoop = "wpi/math/system/LinearSystemLoop.hpp"
# LinearSystemUtil = "wpi/math/system/LinearSystemUtil.hpp"
Models = "wpi/math/system/Models.hpp"
# NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp"
# NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp"
NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp"
NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp"
# wpi/math/trajectory
ExponentialProfile = "wpi/math/trajectory/ExponentialProfile.hpp"

24
wpimath/src/main/python/semiwrap/LinearSystem.yml
View File

@@ -25,9 +25,31 @@ classes:
CalculateX:
CalculateY:
Slice:
# TODO?
ignore: true
template_inline_code: |
cls_LinearSystem.def("slice", [](wpi::math::LinearSystem<States, Inputs, Outputs> &self, int idx0) {
return self.Slice(idx0);
}, py::arg("outputIndex"),
py::doc("Returns the LinearSystem with the outputs listed in outputIndices.")
);
if constexpr (Outputs > 1) {
cls_LinearSystem.def("slice", [](wpi::math::LinearSystem<States, Inputs, Outputs> &self, int idx0, int idx1) {
return self.Slice(idx0, idx1);
}, py::arg("outputIndex0"), py::arg("outputIndex1"),
py::doc("Returns the LinearSystem with the outputs listed in outputIndices.")
);
}
if constexpr (Outputs > 2) {
cls_LinearSystem.def("slice", [](wpi::math::LinearSystem<States, Inputs, Outputs> &self, int idx0, int idx1, int idx2) {
return self.Slice(idx0, idx1, idx2);
}, py::arg("outputIndex0"), py::arg("outputIndex1"), py::arg("outputIndex2"),
py::doc("Returns the LinearSystem with the outputs listed in outputIndices.")
);
}
templates:
LinearSystem_1_1_1:
qualname: wpi::math::LinearSystem

25
wpimath/src/main/python/semiwrap/NumericalIntegration.yml Normal file
View File

@@ -0,0 +1,25 @@
extra_includes:
- frc_eigen.h
- wpi/math/linalg/EigenCore.hpp
- pybind11/functional.h
functions:
RK4:
overloads:
F&&, T, wpi::units::second_t:
template_impls:
- [std::function<Eigen::MatrixXd(Eigen::MatrixXd)>, Eigen::MatrixXd]
F&&, T, U, wpi::units::second_t:
template_impls:
- ["std::function<Eigen::MatrixXd(Eigen::MatrixXd, Eigen::MatrixXd)>", Eigen::MatrixXd, Eigen::MatrixXd]
F&&, wpi::units::second_t, T, wpi::units::second_t:
template_impls:
- ["std::function<Eigen::MatrixXd(wpi::units::second_t, Eigen::MatrixXd)>", Eigen::MatrixXd]
RKDP:
overloads:
F&&, T, U, wpi::units::second_t, double:
template_impls:
- ["std::function<Eigen::MatrixXd(Eigen::MatrixXd, Eigen::MatrixXd)>", Eigen::MatrixXd, Eigen::MatrixXd]
F&&, wpi::units::second_t, T, wpi::units::second_t, double:
template_impls:
- ["std::function<Eigen::MatrixXd(wpi::units::second_t, Eigen::MatrixXd)>", Eigen::MatrixXd]

52
wpimath/src/main/python/semiwrap/NumericalJacobian.yml Normal file
View File

@@ -0,0 +1,52 @@
extra_includes:
- frc_eigen.h
- wpi/math/linalg/EigenCore.hpp
- pybind11/functional.h
- pybind11/typing.h
functions:
NumericalJacobian:
overloads:
F&&, const Vectord<Cols>&:
ignore: true
F&&, const Eigen::VectorXd&:
template_impls:
- [std::function<Eigen::VectorXd(Eigen::VectorXd)>]
NumericalJacobianX:
overloads:
F&&, const Vectord<States>&, const Vectord<Inputs>&, Args&&...:
ignore: true
F&&, const Eigen::VectorXd&, const Eigen::VectorXd&, Args&&...:
# template_impls:
# - ["std::function<Eigen::VectorXd(Eigen::VectorXd, Eigen::VectorXd, py::args)>", py::args, Eigen::MatrixXd]
param_override:
args:
ignore: true
no_release_gil: true
cpp_code: |
[](py::typing::Callable<Eigen::VectorXd(Eigen::VectorXd, Eigen::VectorXd, py::args)> fn,
const Eigen::VectorXd& x, const Eigen::VectorXd& u, py::args args) {
return wpi::math::NumericalJacobianX([=](const Eigen::VectorXd &ix, const Eigen::VectorXd &iu) {
py::object r = fn(ix, iu, *args);
return r.cast<Eigen::VectorXd>();
}, x, u);
}
NumericalJacobianU:
overloads:
F&&, const Vectord<States>&, const Vectord<Inputs>&, Args&&...:
ignore: true
F&&, const Eigen::VectorXd&, const Eigen::VectorXd&, Args&&...:
# template_impls:
# - ["std::function<Eigen::VectorXd(Eigen::VectorXd, Eigen::VectorXd, py::args)>", py::args, Eigen::MatrixXd]
param_override:
args:
ignore: true
no_release_gil: true
cpp_code: |-
[](py::typing::Callable<Eigen::VectorXd(Eigen::VectorXd, Eigen::VectorXd, py::args)> fn,
const Eigen::VectorXd& x, const Eigen::VectorXd& u, py::args args) {
return wpi::math::NumericalJacobianU([=](const Eigen::VectorXd &ix, const Eigen::VectorXd &iu) {
py::object r = fn(ix, iu, *args);
return r.cast<Eigen::VectorXd>();
}, x, u);
}

8
wpimath/src/main/python/tools/create_units.py
View File

@@ -51,9 +51,7 @@ for f in sorted(pathlib.Path(sys.argv[1]).glob("*.h")):
ofp.write("\nnamespace pybind11 { namespace detail {\n")
for single, double in names:
ofp.write(
inspect.cleandoc(
f"""
ofp.write(inspect.cleandoc(f"""
template <> struct handle_type_name<units::{single}_t> {{
static constexpr auto name = _("{double}");
@@ -63,9 +61,7 @@ for f in sorted(pathlib.Path(sys.argv[1]).glob("*.h")):
static constexpr auto name = _("{double}");
}};
"""
)
)
"""))
ofp.write("\n\n")
ofp.write("\n}\n}\n\n")

10
wpimath/src/main/python/wpimath/__init__.py
View File

@@ -101,6 +101,8 @@ from ._wpimath import (
ProfiledPIDControllerRadians,
Quaternion,
QuinticHermiteSpline,
RK4,
RKDP,
Rectangle2d,
RectangularRegionConstraint,
Rotation2d,
@@ -184,6 +186,9 @@ from ._wpimath import (
angleModulus,
applyDeadband,
inputModulus,
numericalJacobian,
numericalJacobianU,
numericalJacobianX,
objectToRobotPose,
slewRateLimit,
)
@@ -288,6 +293,8 @@ __all__ = [
"ProfiledPIDControllerRadians",
"Quaternion",
"QuinticHermiteSpline",
"RK4",
"RKDP",
"Rectangle2d",
"RectangularRegionConstraint",
"Rotation2d",
@@ -371,6 +378,9 @@ __all__ = [
"angleModulus",
"applyDeadband",
"inputModulus",
"numericalJacobian",
"numericalJacobianU",
"numericalJacobianX",
"objectToRobotPose",
"slewRateLimit",
]

8
wpimath/src/test/python/geometry/test_rotation3d.py
View File

@@ -73,7 +73,13 @@ def test_init_rotation_matrix():
assert expected2 == rot2
# Matrix that isn't orthogonal
R3 = np.array([[1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]])
R3 = np.array(
[
[1.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
]
)
with pytest.raises(ValueError):
Rotation3d(R3)

202
wpimath/src/test/python/test_system.py Normal file
View File

@@ -0,0 +1,202 @@
import math
import wpimath
import pytest
import numpy as np
def test_rk4_exponential():
"""Test that integrating dx/dt = eˣ works"""
y0 = np.array([[0.0]])
y1 = wpimath.RK4(lambda x: np.array([[math.exp(x[0, 0])]]), y0, 0.1)
assert math.isclose(y1[0, 0], math.exp(0.1) - math.exp(0.0), abs_tol=1e-3)
def test_rk4_exponential_with_u():
"""Test that integrating dx/dt = eˣ works when we provide a u"""
y0 = np.array([[0.0]])
y1 = wpimath.RK4(
lambda x, u: np.array([[math.exp(u[0, 0] * x[0, 0])]]),
y0,
np.array([[1.0]]),
0.1,
)
assert math.isclose(y1[0, 0], math.exp(0.1) - math.exp(0.0), abs_tol=1e-3)
def test_rk4_time_varying():
"""
Tests RK4 with a time varying solution. From
http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
dx/dt = x (2 / (eᵗ + 1) - 1)
The true (analytical) solution is:
x(t) = 12eᵗ/(eᵗ + 1)²
"""
y0 = np.array([[12.0 * math.exp(5.0) / math.pow(math.exp(5.0) + 1.0, 2.0)]])
y1 = wpimath.RK4(
lambda t, x: np.array([[x[0, 0] * (2.0 / (math.exp(t) + 1.0) - 1.0)]]),
5.0,
y0,
1.0,
)
expected = 12.0 * math.exp(6.0) / math.pow(math.exp(6.0) + 1.0, 2.0)
assert math.isclose(y1[0, 0], expected, abs_tol=1e-3)
def test_rkdp_zero():
"""Tests that integrating dx/dt = 0 works with RKDP"""
y1 = wpimath.RKDP(
lambda x, u: np.zeros((1, 1)),
np.array([[0.0]]),
np.array([[0.0]]),
0.1,
)
assert math.isclose(y1[0, 0], 0.0, abs_tol=1e-3)
def test_rkdp_exponential():
"""Tests that integrating dx/dt = eˣ works with RKDP"""
y0 = np.array([[0.0]])
y1 = wpimath.RKDP(
lambda x, u: np.array([[math.exp(x[0, 0])]]),
y0,
np.array([[0.0]]),
0.1,
)
assert math.isclose(y1[0, 0], math.exp(0.1) - math.exp(0.0), abs_tol=1e-3)
def test_rkdp_time_varying():
"""
Tests RKDP with a time varying solution. From
http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
dx/dt = x(2/(eᵗ + 1) - 1)
The true (analytical) solution is:
x(t) = 12eᵗ/(eᵗ + 1)²
"""
y0 = np.array([[12.0 * math.exp(5.0) / math.pow(math.exp(5.0) + 1.0, 2.0)]])
y1 = wpimath.RKDP(
lambda t, x: np.array([[x[0, 0] * (2.0 / (math.exp(t) + 1.0) - 1.0)]]),
5.0,
y0,
1.0,
1e-12,
)
expected = 12.0 * math.exp(6.0) / math.pow(math.exp(6.0) + 1.0, 2.0)
assert math.isclose(y1[0, 0], expected, abs_tol=1e-3)
def test_numerical_jacobian():
"""Test that we can recover A from ax_fn() pretty accurately"""
a = np.array(
[
[1.0, 2.0, 4.0, 1.0],
[5.0, 2.0, 3.0, 4.0],
[5.0, 1.0, 3.0, 2.0],
[1.0, 1.0, 3.0, 7.0],
]
)
def ax_fn(x):
return a @ x
new_a = wpimath.numericalJacobian(ax_fn, np.zeros((4, 1)))
np.testing.assert_allclose(new_a, a, rtol=1e-6, atol=1e-5)
def test_numerical_jacobian_x_u_square():
"""Test that we can recover B from axbu_fn() pretty accurately"""
a = np.array(
[
[1.0, 2.0, 4.0, 1.0],
[5.0, 2.0, 3.0, 4.0],
[5.0, 1.0, 3.0, 2.0],
[1.0, 1.0, 3.0, 7.0],
]
)
b = np.array([[1.0, 1.0], [2.0, 1.0], [3.0, 2.0], [3.0, 7.0]])
def axbu_fn(x, u):
return a @ x + b @ u
x0 = np.zeros((4, 1))
u0 = np.zeros((2, 1))
new_a = wpimath.numericalJacobianX(axbu_fn, x0, u0)
new_b = wpimath.numericalJacobianU(axbu_fn, x0, u0)
np.testing.assert_allclose(new_a, a, rtol=1e-6, atol=1e-5)
np.testing.assert_allclose(new_b, b, rtol=1e-6, atol=1e-5)
def test_numerical_jacobian_x_u_rectangular():
c = np.array(
[
[1.0, 2.0, 4.0, 1.0],
[5.0, 2.0, 3.0, 4.0],
[5.0, 1.0, 3.0, 2.0],
]
)
d = np.array([[1.0, 1.0], [2.0, 1.0], [3.0, 2.0]])
def cxdu_fn(x, u):
return c @ x + d @ u
x0 = np.zeros((4, 1))
u0 = np.zeros((2, 1))
new_c = wpimath.numericalJacobianX(cxdu_fn, x0, u0)
new_d = wpimath.numericalJacobianU(cxdu_fn, x0, u0)
np.testing.assert_allclose(new_c, c, rtol=1e-6, atol=1e-5)
np.testing.assert_allclose(new_d, d, rtol=1e-6, atol=1e-5)
def test_numerical_jacobian_x_passes_extra_args():
a = np.array([[2.0, -1.0], [0.5, 3.0]])
b = np.array([[1.0], [4.0]])
x0 = np.zeros((2, 1))
u0 = np.zeros((1, 1))
seen = {}
def axbu_fn(x, u, scale, bias):
seen["args"] = (scale, bias)
return scale * (a @ x) + bias * (b @ u)
new_a = wpimath.numericalJacobianX(axbu_fn, x0, u0, 2.5, -3.0)
assert seen["args"] == (2.5, -3.0)
np.testing.assert_allclose(new_a, 2.5 * a, rtol=1e-6, atol=1e-5)
def test_numerical_jacobian_u_passes_extra_args():
a = np.array([[1.0, 0.0], [0.0, -2.0]])
b = np.array([[1.5], [-0.5]])
x0 = np.zeros((2, 1))
u0 = np.zeros((1, 1))
seen = {}
def axbu_fn(x, u, scale, bias):
seen["args"] = (scale, bias)
return scale * (a @ x) + bias * (b @ u)
new_b = wpimath.numericalJacobianU(axbu_fn, x0, u0, 4.0, 0.25)
assert seen["args"] == (4.0, 0.25)
np.testing.assert_allclose(new_b, 0.25 * b, rtol=1e-6, atol=1e-5)