From b3526ded7776111566e330887ed0dc2af71b538f Mon Sep 17 00:00:00 2001 From: Meow404 Date: Mon, 9 Mar 2026 20:34:18 -0400 Subject: [PATCH 1/9] fix: breaking python tests --- bindings/pyc3/c3_multibody_py.cc | 11 ++++- bindings/pyc3/c3_py.cc | 12 +++++- bindings/pyc3/c3_systems_py.cc | 7 ++++ examples/python/lcs_factory_example.py | 42 +++---------------- examples/python/lcs_factory_system_example.py | 34 --------------- 5 files changed, 33 insertions(+), 73 deletions(-) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 0560d07..3c24819 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -36,6 +36,13 @@ PYBIND11_MODULE(multibody, m) { const c3::LCSFactoryOptions&>(), py::arg("plant"), py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options")) + .def(py::init&, + drake::systems::Context&, + const drake::multibody::MultibodyPlant&, + drake::systems::Context&, + c3::LCSFactoryOptions&>(), + py::arg("plant"), py::arg("context"), py::arg("plant_ad"), + py::arg("context_ad"), py::arg("options")) .def("GenerateLCS", &c3::multibody::LCSFactory::GenerateLCS) .def("GetContactJacobianAndPoints", &c3::multibody::LCSFactory::GetContactJacobianAndPoints) @@ -77,7 +84,9 @@ PYBIND11_MODULE(multibody, m) { .def_readwrite("num_friction_directions", &ContactPairConfig::num_friction_directions) .def_readwrite("planar_normal_direction", - &ContactPairConfig::planar_normal_direction); + &ContactPairConfig::planar_normal_direction) + .def_readwrite("num_active_contact_pairs", + &ContactPairConfig::num_active_contact_pairs); py::class_(m, "LCSFactoryOptions") .def(py::init<>()) diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index 703c58f..f5ac91c 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -132,6 +132,15 @@ PYBIND11_MODULE(c3, m) { py::arg("LCS"), py::arg("costs"), py::arg("x_desired"), py::arg("options")); + py::class_(m, "LCSSimulateConfig") + .def(py::init<>()) + .def_readwrite("regularized", &LCSSimulateConfig::regularized) + .def_readwrite("piv_tol", &LCSSimulateConfig::piv_tol) + .def_readwrite("zero_tol", &LCSSimulateConfig::zero_tol) + .def_readwrite("min_exp", &LCSSimulateConfig::min_exp) + .def_readwrite("step_exp", &LCSSimulateConfig::step_exp) + .def_readwrite("max_exp", &LCSSimulateConfig::max_exp); + py::class_(m, "LCS") .def(py::init&, const std::vector&, @@ -154,7 +163,8 @@ PYBIND11_MODULE(c3, m) { py::arg("dt")) .def(py::init(), py::arg("other")) .def("Simulate", &LCS::Simulate, py::arg("x_init"), py::arg("u"), - py::arg("regularized") = false, "Simulate the system for one step") + py::arg("simulate_config") = LCSSimulateConfig(), + "Simulate the system for one step") .def("A", &LCS::A, py::return_value_policy::copy) .def("B", &LCS::B, py::return_value_policy::copy) .def("D", &LCS::D, py::return_value_policy::copy) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index ebeb288..364a392 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -92,6 +92,13 @@ PYBIND11_MODULE(systems, m) { LCSFactoryOptions>(), py::arg("plant"), py::arg("context"), py::arg("plant_ad"), py::arg("context_ad"), py::arg("contact_geoms"), py::arg("options")) + .def(py::init&, + drake::systems::Context&, + const MultibodyPlant&, + drake::systems::Context&, + LCSFactoryOptions&>(), + py::arg("plant"), py::arg("context"), py::arg("plant_ad"), + py::arg("context_ad"), py::arg("options")) .def("get_input_port_lcs_state", &LCSFactorySystem::get_input_port_lcs_state, py::return_value_policy::reference) diff --git a/examples/python/lcs_factory_example.py b/examples/python/lcs_factory_example.py index 879df74..168b417 100644 --- a/examples/python/lcs_factory_example.py +++ b/examples/python/lcs_factory_example.py @@ -29,47 +29,16 @@ def make_cube_pivoting_lcs_plant(): plant_diagram = builder.Build() plant_diagram_context = plant_diagram.CreateDefaultContext() - # Retrieve collision geometries for relevant bodies. - platform_collision_geoms = plant.GetCollisionGeometriesForBody( - plant.GetBodyByName("platform") - ) - cube_collision_geoms = plant.GetCollisionGeometriesForBody( - plant.GetBodyByName("cube") - ) - left_finger_collision_geoms = plant.GetCollisionGeometriesForBody( - plant.GetBodyByName("left_finger") - ) - right_finger_collision_geoms = plant.GetCollisionGeometriesForBody( - plant.GetBodyByName("right_finger") - ) - - # Map collision geometries to their respective components. - contact_geoms = {} - contact_geoms["PLATFORM"] = platform_collision_geoms - contact_geoms["CUBE"] = cube_collision_geoms - contact_geoms["LEFT_FINGER"] = left_finger_collision_geoms - contact_geoms["RIGHT_FINGER"] = right_finger_collision_geoms - - # Define contact pairs for the LCS system. - contact_pairs = [] - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["LEFT_FINGER"][0]]) - ) - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["PLATFORM"][0]]) - ) - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["RIGHT_FINGER"][0]]) - ) - - return builder, plant_diagram, plant_diagram_context, plant, contact_pairs + return builder, plant_diagram, plant_diagram_context, plant def main(): - c3_controller_options = LoadC3ControllerOptions("examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml") + c3_controller_options = LoadC3ControllerOptions( + "examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml" + ) c3_options = c3_controller_options.c3_options lcs_factory_options = c3_controller_options.lcs_factory_options - _, diagram, diagram_context, plant, contact_pairs = make_cube_pivoting_lcs_plant() + _, diagram, diagram_context, plant = make_cube_pivoting_lcs_plant() plant_autodiff = System.ToAutoDiffXd(plant) plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context) @@ -79,7 +48,6 @@ def main(): plant_context, plant_autodiff, plant_autodiff_context, - contact_pairs, lcs_factory_options, ) diff --git a/examples/python/lcs_factory_system_example.py b/examples/python/lcs_factory_system_example.py index 92ee7ca..ecf8694 100644 --- a/examples/python/lcs_factory_system_example.py +++ b/examples/python/lcs_factory_system_example.py @@ -259,39 +259,6 @@ def RunPivotingTest(): # Build the plant diagram. plant_diagram = builder.Build() - # Retrieve collision geometries for relevant bodies. - platform_collision_geoms = plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("platform") - ) - cube_collision_geoms = plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("cube") - ) - left_finger_collision_geoms = plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("left_finger") - ) - right_finger_collision_geoms = plant_for_lcs.GetCollisionGeometriesForBody( - plant_for_lcs.GetBodyByName("right_finger") - ) - - # Map collision geometries to their respective components. - contact_geoms = {} - contact_geoms["PLATFORM"] = platform_collision_geoms - contact_geoms["CUBE"] = cube_collision_geoms - contact_geoms["LEFT_FINGER"] = left_finger_collision_geoms - contact_geoms["RIGHT_FINGER"] = right_finger_collision_geoms - - # Define contact pairs for the LCS system. - contact_pairs = [] - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["LEFT_FINGER"][0]]) - ) - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["PLATFORM"][0]]) - ) - contact_pairs.append( - tuple([contact_geoms["CUBE"][0], contact_geoms["RIGHT_FINGER"][0]]) - ) - # Build the main diagram. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) @@ -323,7 +290,6 @@ def RunPivotingTest(): plant_for_lcs_context, plant_autodiff, plant_context_autodiff, - contact_pairs, options.lcs_factory_options, ) ) From 56f5797f825cb69788b50551073c885a61bbce5f Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 00:17:26 -0400 Subject: [PATCH 2/9] test: add smoke tests for bindings --- bindings/pyc3/BUILD.bazel | 6 +- bindings/pyc3/c3_multibody_py.cc | 26 +- bindings/pyc3/c3_py.cc | 33 ++- bindings/pyc3/c3_systems_py.cc | 1 + bindings/pyc3/test/BUILD.bazel | 61 +++++ bindings/pyc3/test/test_c3.py | 391 +++++++++++++++++++++++++++ bindings/pyc3/test/test_multibody.py | 147 ++++++++++ bindings/pyc3/test/test_systems.py | 247 +++++++++++++++++ bindings/pyc3/test_c3_smoke.py | 285 +++++++++++++++++++ core/c3.cc | 50 ++-- 10 files changed, 1221 insertions(+), 26 deletions(-) create mode 100644 bindings/pyc3/test/BUILD.bazel create mode 100644 bindings/pyc3/test/test_c3.py create mode 100644 bindings/pyc3/test/test_multibody.py create mode 100644 bindings/pyc3/test/test_systems.py create mode 100644 bindings/pyc3/test_c3_smoke.py diff --git a/bindings/pyc3/BUILD.bazel b/bindings/pyc3/BUILD.bazel index ced0886..3d8b66b 100644 --- a/bindings/pyc3/BUILD.bazel +++ b/bindings/pyc3/BUILD.bazel @@ -15,11 +15,15 @@ load("@rules_python//python:packaging.bzl", "py_wheel") pybind_py_library( name = "c3_py", - cc_deps = ["//core:c3"], + cc_deps = [ + "//core:c3", + "@drake//:drake_shared_library", + ], cc_so_name = "c3", cc_srcs = ["c3_py.cc"], py_deps = [ ":module_py", + "@drake//bindings/pydrake", ], py_imports = ["."], ) diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 3c24819..633f61a 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -92,7 +92,31 @@ PYBIND11_MODULE(multibody, m) { .def(py::init<>()) .def_readwrite("dt", &LCSFactoryOptions::dt) .def_readwrite("N", &LCSFactoryOptions::N) - .def_readwrite("contact_model", &LCSFactoryOptions::contact_model) + .def_property( + "contact_model", + [](const LCSFactoryOptions& self) { + // Convert string back to enum for Python + if (self.contact_model == "stewart_and_trinkle") + return c3::multibody::ContactModel::kStewartAndTrinkle; + if (self.contact_model == "anitescu") + return c3::multibody::ContactModel::kAnitescu; + if (self.contact_model == "frictionless_spring") + return c3::multibody::ContactModel::kFrictionlessSpring; + return c3::multibody::ContactModel::kUnknown; + }, + [](LCSFactoryOptions& self, c3::multibody::ContactModel val) { + // Convert enum to the string the C++ struct expects + switch (val) { + case c3::multibody::ContactModel::kStewartAndTrinkle: + self.contact_model = "stewart_and_trinkle"; break; + case c3::multibody::ContactModel::kAnitescu: + self.contact_model = "anitescu"; break; + case c3::multibody::ContactModel::kFrictionlessSpring: + self.contact_model = "frictionless_spring"; break; + default: + self.contact_model = "unknown"; break; + } + }) .def_readwrite("num_friction_directions", &LCSFactoryOptions::num_friction_directions) .def_readwrite("num_friction_directions_per_contact", diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index f5ac91c..144972a 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -6,6 +6,7 @@ #include "core/c3.h" #include "core/c3_miqp.h" #include "core/c3_options.h" +#include "core/c3_plus.h" #include "core/c3_qp.h" #include "core/lcs.h" @@ -86,13 +87,24 @@ PYBIND11_MODULE(c3, m) { return self.Solve(x0); }) .def("UpdateLCS", &C3::UpdateLCS, py::arg("lcs")) - .def("GetDynamicConstraints", &C3::GetDynamicConstraints, - py::return_value_policy::copy) + .def( + "GetDynamicConstraints", + [](C3& self) { + py::module_::import("pydrake.solvers"); + return self.GetDynamicConstraints(); + }, + py::return_value_policy::reference_internal) .def("UpdateTarget", &C3::UpdateTarget, py::arg("x_des")) .def("UpdateCostMatrices", &C3::UpdateCostMatrices, py::arg("costs")) .def("GetCostMatrices", &C3::GetCostMatrices, py::return_value_policy::copy) - .def("GetTargetCost", &C3::GetTargetCost, py::return_value_policy::copy) + .def( + "GetTargetCost", + [](C3& self) { + py::module_::import("pydrake.solvers"); + return self.GetTargetCost(); + }, + py::return_value_policy::reference_internal) .def("AddLinearConstraint", static_cast(m, "C3Plus") + .def(py::init&, const C3Options&>(), + py::arg("LCS"), py::arg("costs"), py::arg("x_desired"), + py::arg("options")); + py::class_(m, "LCSSimulateConfig") .def(py::init<>()) .def_readwrite("regularized", &LCSSimulateConfig::regularized) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index 364a392..0c53b34 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -35,6 +35,7 @@ namespace systems { namespace pyc3 { PYBIND11_MODULE(systems, m) { py::module::import("pydrake.systems.framework"); + py::module::import("multibody"); // ensure LCSFactoryOptions is registered py::class_>(m, "C3Controller") .def(py::init&, const C3::CostMatrices, C3ControllerOptions>(), diff --git a/bindings/pyc3/test/BUILD.bazel b/bindings/pyc3/test/BUILD.bazel new file mode 100644 index 0000000..8037e5c --- /dev/null +++ b/bindings/pyc3/test/BUILD.bazel @@ -0,0 +1,61 @@ +# -*- python -*- +package(default_visibility = ["//visibility:public"]) + +py_test( + name = "test_c3", + srcs = ["test_c3.py"], + deps = [ + "//bindings/pyc3:c3_py", + "@rules_python//python/runfiles", + ], + data = [ + "//core:test_data", + "//examples:example_data", + ], + python_version = "PY3", + tags = ["smoke"], + env_inherit = [ + "GUROBI_HOME", + "GRB_LICENSE_FILE", + ], +) + +py_test( + name = "test_systems", + srcs = ["test_systems.py"], + deps = [ + "//bindings/pyc3:c3_py", + "//bindings/pyc3:c3_systems_py", + "//bindings/pyc3:c3_multibody_py", + "@rules_python//python/runfiles", + ], + data = [ + "//examples:example_data", + ], + python_version = "PY3", + tags = ["smoke"], + env_inherit = [ + "GUROBI_HOME", + "GRB_LICENSE_FILE", + ], +) + +py_test( + name = "test_multibody", + srcs = ["test_multibody.py"], + deps = [ + "//bindings/pyc3:c3_py", + "//bindings/pyc3:c3_multibody_py", + "@rules_python//python/runfiles", + ], + data = [ + "//multibody:test_data", + "//examples:example_data", + ], + python_version = "PY3", + tags = ["smoke"], + env_inherit = [ + "GUROBI_HOME", + "GRB_LICENSE_FILE", + ], +) diff --git a/bindings/pyc3/test/test_c3.py b/bindings/pyc3/test/test_c3.py new file mode 100644 index 0000000..878b3c2 --- /dev/null +++ b/bindings/pyc3/test/test_c3.py @@ -0,0 +1,391 @@ +"""Smoke tests for c3 core bindings.""" + +import copy +import os +import unittest +from scipy import linalg + +import numpy as np +import c3 + + +def _data_path(relative): + """Resolve a runfiles data path relative to the workspace root.""" + return os.path.join(os.environ.get("TEST_SRCDIR", "."), "_main", relative) + + +def make_cartpole_lcs(N=5, dt=0.01): + g = 9.81 + mp = 0.411 + mc = 0.978 + len_p = 0.6 + len_com = 0.4267 + d1 = 0.35 + d2 = -0.35 + ks = 100 + A = np.array( + [ + [0, 0, 1, 0], + [0, 0, 0, 1], + [0, g * mp / mc, 0, 0], + [0, g * (mc + mp) / (len_com * mc), 0, 0], + ] + ) + A = np.eye(4) + dt * A + B = dt * np.array([[0], [0], [1 / mc], [1 / (len_com * mc)]]) + D = dt * np.array( + [ + [0, 0], + [0, 0], + [(-1 / mc) + (len_p / (mc * len_com)), (1 / mc) - (len_p / (mc * len_com))], + [ + (-1 / (mc * len_com)) + + (len_p * (mc + mp)) / (mc * mp * len_com * len_com), + -( + (-1 / (mc * len_com)) + + (len_p * (mc + mp)) / (mc * mp * len_com * len_com) + ), + ], + ] + ) + E = np.array([[-1, len_p, 0, 0], [1, -len_p, 0, 0]]) + F = (1.0 / ks) * np.eye(2) + c_vec = np.array([d1, -d2]) + d = np.zeros(4) + H = np.zeros((2, 1)) + return c3.LCS(A, B, D, d, E, F, H, c_vec, N, dt) + + +def make_cartpole_options_and_costs(lcs, N=5, c3plus=False): + n_x = lcs.num_states() # 4 + n_u = lcs.num_inputs() # 1 + n_lambda = lcs.num_lambdas() # 2 + + # State cost: penalize cart pos, pole angle, velocities + Q_diag = np.diag([10.0, 2.0, 1.0, 1.0]) + # Terminal cost via DARE + Q_f = linalg.solve_discrete_are(lcs.A()[0], lcs.B()[0], Q_diag, np.eye(n_u)) + R_mat = np.eye(n_u) + + n_z = n_x + n_lambda + n_u + if c3plus: + n_z += n_lambda # extra eta terms for C3+ + G_mat = np.diag( + [0.1] * n_x + [0.1] * n_lambda + [0.0] * n_u + + ([1.0] * n_lambda if c3plus else []) + ) + U_mat = np.diag( + [1000.0] * n_x + [1.0] * n_lambda + [0.0] * n_u + + ([10000.0] * n_lambda if c3plus else []) + ) + + Q_list = [Q_diag] * N + [Q_f] + R_list = [R_mat] * N + G_list = [G_mat] * N + U_list = [U_mat] * N + + costs = c3.CostMatrices(Q_list, R_list, G_list, U_list) + + opts = c3.C3Options() + opts.Q = Q_diag + opts.R = R_mat + opts.G = G_mat + opts.U = U_mat + opts.g_vector = [0.1] * n_lambda + [0.0] * n_u + opts.u_vector = [1.0] * n_lambda + [0.0] * n_u + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = True + opts.num_threads = 5 + opts.admm_iter = 10 + opts.M = 1000.0 + opts.gamma = 1.0 + opts.rho_scale = 2.0 + opts.delta_option = 0 + + return opts, costs + + +# keep the simple synthetic helpers for non-solver tests +def make_lcs(n_x=4, n_u=2, n_lambda=2, N=3, dt=0.01): + return c3.LCS( + np.eye(n_x), + np.ones((n_x, n_u)), + np.ones((n_x, n_lambda)), + np.ones(n_x), + np.ones((n_lambda, n_x)), + np.eye(n_lambda), + np.ones((n_lambda, n_u)), + np.ones(n_lambda), + N, + dt, + ) + + +def make_options(n_x=4, n_u=2, n_lambda=2, is_c3plus=False): + opts = c3.C3Options() + opts.Q = np.eye(n_x) + opts.R = np.eye(n_u) + n_z = n_x + n_u + n_lambda + (n_lambda if is_c3plus else 0) + opts.G = np.ones((n_z, n_z)) + opts.U = np.ones((n_z, n_z)) + opts.g_vector = [1.0] * n_lambda + opts.u_vector = [1.0] * n_u + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = False + opts.num_threads = 1 + opts.admm_iter = 3 + opts.M = 100.0 + opts.gamma = 0.1 + opts.rho_scale = 1.0 + return opts + + +def _data_path(relative): + """Resolve a runfiles data path relative to the workspace root.""" + return os.path.join(os.environ.get("RUNFILES_DIR", "."), "_main", relative) + + +class TestLCSSimulateConfig(unittest.TestCase): + def test_fields(self): + cfg = c3.LCSSimulateConfig() + cfg.regularized = False + cfg.piv_tol = 1e-8 + cfg.zero_tol = 1e-10 + cfg.min_exp = -4 + cfg.step_exp = 1 + cfg.max_exp = 4 + self.assertFalse(cfg.regularized) + self.assertAlmostEqual(cfg.piv_tol, 1e-8) + + +class TestLCS(unittest.TestCase): + def setUp(self): + self.n_x, self.n_u, self.n_lambda, self.N, self.dt = 4, 2, 2, 3, 0.01 + self.lcs = make_lcs(self.n_x, self.n_u, self.n_lambda, self.N, self.dt) + + def test_dimensions(self): + lcs = self.lcs + self.assertEqual(lcs.num_states(), self.n_x) + self.assertEqual(lcs.num_inputs(), self.n_u) + self.assertEqual(lcs.num_lambdas(), self.n_lambda) + self.assertEqual(lcs.N(), self.N) + self.assertAlmostEqual(lcs.dt(), self.dt) + + def test_accessors(self): + lcs = self.lcs + self.assertEqual(lcs.A()[0].shape, (self.n_x, self.n_x)) + self.assertEqual(lcs.B()[0].shape, (self.n_x, self.n_u)) + self.assertEqual(lcs.D()[0].shape, (self.n_x, self.n_lambda)) + self.assertEqual(lcs.d()[0].shape, (self.n_x,)) + self.assertEqual(lcs.E()[0].shape, (self.n_lambda, self.n_x)) + self.assertEqual(lcs.F()[0].shape, (self.n_lambda, self.n_lambda)) + self.assertEqual(lcs.H()[0].shape, (self.n_lambda, self.n_u)) + self.assertEqual(lcs.c()[0].shape, (self.n_lambda,)) + + def test_setters(self): + n_x, n_u, n_lambda, N = self.n_x, self.n_u, self.n_lambda, self.N + lcs = self.lcs + lcs.set_A([np.eye(n_x)] * N) + lcs.set_B([np.zeros((n_x, n_u))] * N) + lcs.set_D([np.zeros((n_x, n_lambda))] * N) + lcs.set_d([np.zeros(n_x)] * N) + lcs.set_E([np.zeros((n_lambda, n_x))] * N) + lcs.set_F([np.eye(n_lambda)] * N) + lcs.set_H([np.zeros((n_lambda, n_u))] * N) + lcs.set_c([np.ones(n_lambda)] * N) + + def test_simulate(self): + x0 = np.zeros(self.n_x) + u = np.zeros(self.n_u) + x_next = self.lcs.Simulate(x0, u) + self.assertEqual(x_next.shape, (self.n_x,)) + x_next2 = self.lcs.Simulate(x0, u, c3.LCSSimulateConfig()) + self.assertEqual(x_next2.shape, (self.n_x,)) + + def test_list_constructor(self): + n_x, n_u, n_lambda, N, dt = self.n_x, self.n_u, self.n_lambda, self.N, self.dt + lcs2 = c3.LCS( + [np.eye(n_x)] * N, + [np.zeros((n_x, n_u))] * N, + [np.zeros((n_x, n_lambda))] * N, + [np.zeros(n_x)] * N, + [np.zeros((n_lambda, n_x))] * N, + [np.eye(n_lambda)] * N, + [np.zeros((n_lambda, n_u))] * N, + [np.ones(n_lambda)] * N, + dt, + ) + self.assertEqual(lcs2.num_states(), n_x) + + def test_copy_constructors(self): + lcs3 = c3.LCS(self.lcs) + self.assertEqual(lcs3.num_states(), self.n_x) + lcs4 = copy.copy(self.lcs) + self.assertEqual(lcs4.num_states(), self.n_x) + lcs5 = copy.deepcopy(self.lcs) + self.assertEqual(lcs5.num_states(), self.n_x) + + def test_placeholder(self): + lcs_ph = c3.LCS.CreatePlaceholderLCS( + self.n_x, self.n_u, self.n_lambda, self.N, self.dt + ) + self.assertEqual(lcs_ph.num_states(), self.n_x) + + +class TestCostMatrices(unittest.TestCase): + def test_construction_and_properties(self): + n_x, n_u, n_lambda, N = 4, 2, 2, 3 + Q = [np.eye(n_x)] * (N + 1) + R = [np.eye(n_u)] * N + G = [np.eye(n_lambda)] * N + U = [np.eye(n_u)] * N + cm = c3.CostMatrices(Q, R, G, U) + self.assertEqual(len(cm.Q), N + 1) + self.assertEqual(len(cm.R), N) + self.assertEqual(len(cm.G), N) + self.assertEqual(len(cm.U), N) + cm.Q = Q + cm.R = R + cm.G = G + cm.U = U + + def test_default_construction(self): + cm = c3.CostMatrices() + self.assertIsNotNone(cm) + + +class TestC3Options(unittest.TestCase): + def test_fields(self): + opts = c3.C3Options() + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = False + opts.num_threads = 1 + opts.delta_option = 0 + opts.M = 100.0 + opts.admm_iter = 10 + opts.gamma = 0.1 + opts.rho_scale = 1.0 + self.assertFalse(opts.warm_start) + self.assertEqual(opts.admm_iter, 10) + self.assertAlmostEqual(opts.gamma, 0.1) + + def test_matrix_properties(self): + opts = make_options() + np.testing.assert_array_equal(opts.Q, np.eye(4)) + np.testing.assert_array_equal(opts.R, np.eye(2)) + + +class TestConstraintVariable(unittest.TestCase): + def test_values(self): + self.assertIsNotNone(c3.ConstraintVariable.STATE) + self.assertIsNotNone(c3.ConstraintVariable.INPUT) + self.assertIsNotNone(c3.ConstraintVariable.FORCE) + + +class TestC3QP(unittest.TestCase): + def setUp(self): + self.N = 5 + self.lcs = make_cartpole_lcs(self.N) + self.n_x = self.lcs.num_states() + self.n_u = self.lcs.num_inputs() + self.n_lambda = self.lcs.num_lambdas() + self.opts, self.costs = make_cartpole_options_and_costs(self.lcs, self.N) + self.x_des = [np.zeros(self.n_x)] * (self.N + 1) + self.solver = c3.C3QP(self.lcs, self.costs, self.x_des, self.opts) + + def test_solve_and_solutions(self): + self.solver.Solve(np.zeros(self.n_x)) + self.assertIsNotNone(self.solver.GetFullSolution()) + self.assertIsNotNone(self.solver.GetStateSolution()) + self.assertIsNotNone(self.solver.GetForceSolution()) + self.assertIsNotNone(self.solver.GetInputSolution()) + self.assertIsNotNone(self.solver.GetDualDeltaSolution()) + self.assertIsNotNone(self.solver.GetDualWSolution()) + + def test_get_cost_matrices(self): + costs = self.solver.GetCostMatrices() + self.assertEqual(len(costs.Q), self.N + 1) + + def test_get_target_cost(self): + self.solver.Solve(np.zeros(self.n_x)) + result = self.solver.GetTargetCost() + self.assertIsNotNone(result) + + def test_get_dynamic_constraints(self): + self.solver.Solve(np.zeros(self.n_x)) + result = self.solver.GetDynamicConstraints() + self.assertIsNotNone(result) + + def test_get_linear_constraints(self): + result = self.solver.GetLinearConstraints() + self.assertIsNotNone(result) + + def test_update_methods(self): + self.solver.UpdateLCS(self.lcs) + self.solver.UpdateTarget(self.x_des) + self.solver.UpdateCostMatrices(self.costs) + + def test_add_remove_constraints(self): + n_x, n_u, n_lambda = self.n_x, self.n_u, self.n_lambda + self.solver.AddLinearConstraint( + np.eye(n_x), -np.ones(n_x), np.ones(n_x), c3.ConstraintVariable.STATE + ) + self.solver.RemoveConstraints() + self.solver.AddLinearConstraint( + np.eye(n_u), -np.ones(n_u), np.ones(n_u), c3.ConstraintVariable.INPUT + ) + self.solver.RemoveConstraints() + self.solver.AddLinearConstraint( + np.eye(n_lambda), + -np.ones(n_lambda), + np.ones(n_lambda), + c3.ConstraintVariable.FORCE, + ) + self.solver.RemoveConstraints() + + def test_create_cost_matrices_from_options(self): + costs = c3.C3.CreateCostMatricesFromC3Options(self.opts, self.N) + self.assertEqual(len(costs.Q), self.N + 1) + self.assertEqual(len(costs.R), self.N) + + +class TestC3MIQP(unittest.TestCase): + def test_solve(self): + N = 5 + lcs = make_cartpole_lcs(N) + opts, costs = make_cartpole_options_and_costs(lcs, N) + n_x = lcs.num_states() + solver = c3.C3MIQP(lcs, costs, [np.zeros(n_x)] * (N + 1), opts) + solver.Solve(np.zeros(n_x)) + self.assertIsNotNone(solver.GetStateSolution()) + self.assertIsNotNone(solver.GetForceSolution()) + self.assertIsNotNone(solver.GetInputSolution()) + + +class TestC3Plus(unittest.TestCase): + def test_solve(self): + N = 5 + lcs = make_cartpole_lcs(N) + opts, costs = make_cartpole_options_and_costs(lcs, N, c3plus=True) + n_x = lcs.num_states() + solver = c3.C3Plus(lcs, costs, [np.zeros(n_x)] * (N + 1), opts) + solver.Solve(np.zeros(n_x)) + self.assertIsNotNone(solver.GetStateSolution()) + self.assertIsNotNone(solver.GetForceSolution()) + self.assertIsNotNone(solver.GetInputSolution()) + + +class TestLoadC3Options(unittest.TestCase): + def test_load(self): + fname = _data_path("core/test/resources/c3_cartpole_options.yaml") + opts = c3.LoadC3Options(fname) + self.assertIsNotNone(opts) + self.assertGreater(opts.admm_iter, 0) + self.assertGreater(opts.M, 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/bindings/pyc3/test/test_multibody.py b/bindings/pyc3/test/test_multibody.py new file mode 100644 index 0000000..ed4565c --- /dev/null +++ b/bindings/pyc3/test/test_multibody.py @@ -0,0 +1,147 @@ +"""Smoke tests for c3 multibody bindings.""" + +import unittest +import multibody +import os + + +def _data_path(relative): + return os.path.join(os.environ.get("RUNFILES_DIR", "."), "_main", relative) + + +class TestContactModel(unittest.TestCase): + def test_values(self): + self.assertIsNotNone(multibody.ContactModel.Unknown) + self.assertIsNotNone(multibody.ContactModel.StewartAndTrinkle) + self.assertIsNotNone(multibody.ContactModel.Anitescu) + self.assertIsNotNone(multibody.ContactModel.FrictionlessSpring) + + +class TestLCSFactoryOptions(unittest.TestCase): + def test_fields(self): + opts = multibody.LCSFactoryOptions() + opts.dt = 0.01 + opts.N = 3 + opts.num_contacts = 2 + # mu is list[float] per binding + opts.mu = [0.5] + opts.spring_stiffness = 100.0 + opts.num_friction_directions = 4 + self.assertAlmostEqual(opts.dt, 0.01) + self.assertEqual(opts.N, 3) + self.assertEqual(opts.num_contacts, 2) + self.assertAlmostEqual(opts.mu[0], 0.5) + + def test_contact_model(self): + opts = multibody.LCSFactoryOptions() + opts.contact_model = multibody.ContactModel.StewartAndTrinkle + self.assertEqual(opts.contact_model, multibody.ContactModel.StewartAndTrinkle) + + def test_contact_pair_configs(self): + opts = multibody.LCSFactoryOptions() + cfg = multibody.ContactPairConfig() + opts.contact_pair_configs = [cfg] + self.assertEqual(len(opts.contact_pair_configs), 1) + + +class TestContactPairConfig(unittest.TestCase): + def test_fields(self): + cfg = multibody.ContactPairConfig() + cfg.mu = 0.7 + cfg.num_friction_directions = 2 + cfg.num_active_contact_pairs = 1 + self.assertAlmostEqual(cfg.mu, 0.7) + self.assertEqual(cfg.num_friction_directions, 2) + + def test_body_fields(self): + cfg = multibody.ContactPairConfig() + cfg.body_A = "base" + cfg.body_B = "link1" + self.assertEqual(cfg.body_A, "base") + self.assertEqual(cfg.body_B, "link1") + + def test_geom_indices(self): + cfg = multibody.ContactPairConfig() + cfg.body_A_collision_geom_indices = [0, 1] + cfg.body_B_collision_geom_indices = [2] + self.assertEqual(cfg.body_A_collision_geom_indices, [0, 1]) + + +class TestLCSFactoryGetNumContactVariables(unittest.TestCase): + def test_with_contact_model(self): + n = multibody.LCSFactory.GetNumContactVariables( + multibody.ContactModel.StewartAndTrinkle, 2, 4 + ) + self.assertGreater(n, 0) + + def test_with_options(self): + opts = multibody.LCSFactoryOptions() + opts.num_contacts = 2 + opts.num_friction_directions = 4 + opts.contact_model = multibody.ContactModel.StewartAndTrinkle + n = multibody.LCSFactory.GetNumContactVariables(opts) + self.assertGreater(n, 0) + + +class TestLoadLCSFactoryOptions(unittest.TestCase): + def test_load(self): + opts = multibody.LoadLCSFactoryOptions( + "multibody/test/resources/lcs_factory_pivoting_options.yaml" + ) + self.assertEqual(opts.N, 10) + self.assertAlmostEqual(opts.dt, 0.01) + self.assertEqual(opts.num_contacts, 3) + self.assertEqual(opts.contact_model, multibody.ContactModel.StewartAndTrinkle) + self.assertEqual(opts.num_friction_directions, 1) + self.assertAlmostEqual(opts.mu[0], 0.1) + self.assertEqual(len(opts.contact_pair_configs), 3) + self.assertEqual(opts.contact_pair_configs[0].body_A, "cube") + self.assertEqual(opts.contact_pair_configs[0].body_B, "left_finger") + + def test_get_num_contact_variables_from_loaded_options(self): + opts = multibody.LoadLCSFactoryOptions( + "multibody/test/resources/lcs_factory_pivoting_options.yaml" + ) + n = multibody.LCSFactory.GetNumContactVariables(opts) + self.assertGreater(n, 0) + + +class TestLCSFactoryCallable(unittest.TestCase): + """Verify LCSFactory methods are callable (may raise due to missing plant).""" + + def test_fix_some_modes_callable(self): + import c3 + import numpy as np + + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = c3.LCS( + np.eye(n_x), + np.ones((n_x, n_u)), + np.ones((n_x, n_lambda)), + np.ones(n_x), + np.ones((n_lambda, n_x)), + np.eye(n_lambda), + np.ones((n_lambda, n_u)), + np.ones(n_lambda), + N, + dt, + ) + # FixSomeModes takes sets of ints — just call it + result = multibody.LCSFactory.FixSomeModes(lcs, {0}, {1}) + self.assertIsNotNone(result) + + def test_get_num_contact_variables_anitescu(self): + n = multibody.LCSFactory.GetNumContactVariables( + multibody.ContactModel.Anitescu, 2, 4 + ) + self.assertGreater(n, 0) + + def test_get_num_contact_variables_frictionless(self): + n = multibody.LCSFactory.GetNumContactVariables( + multibody.ContactModel.FrictionlessSpring, 2, 0 + ) + self.assertGreater(n, 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/bindings/pyc3/test/test_systems.py b/bindings/pyc3/test/test_systems.py new file mode 100644 index 0000000..fbba541 --- /dev/null +++ b/bindings/pyc3/test/test_systems.py @@ -0,0 +1,247 @@ +"""Smoke tests for c3 systems bindings.""" + +import copy +import os +import tempfile +import unittest + +import numpy as np +import c3 +import systems + +# multibody is a separate module — import it so LCSFactoryOptions is available +try: + import multibody + + HAS_MULTIBODY = True +except ImportError: + HAS_MULTIBODY = False + + +def _data_path(relative): + return os.path.join(os.environ.get("RUNFILES_DIR", "."), "_main", relative) + + +class TestC3Solution(unittest.TestCase): + def test_default_construction(self): + sol = systems.C3Solution() + self.assertIsNotNone(sol) + + def test_construction_with_dims(self): + n_x, n_lambda, n_u, N = 4, 2, 2, 3 + sol = systems.C3Solution(n_x, n_lambda, n_u, N) + # Verify collections are non-empty and all elements are 1-D numpy arrays + self.assertGreater(len(sol.x_sol), 0) + self.assertGreater(len(sol.lambda_sol), 0) + self.assertGreater(len(sol.u_sol), 0) + for v in sol.x_sol: + self.assertEqual(v.ndim, 1) + for v in sol.lambda_sol: + self.assertEqual(v.ndim, 1) + for v in sol.u_sol: + self.assertEqual(v.ndim, 1) + + def test_time_vector(self): + sol = systems.C3Solution(4, 2, 2, 3) + self.assertIsNotNone(sol.time_vector) + + def test_readwrite_fields(self): + sol = systems.C3Solution(4, 2, 2, 3) + # overwrite x_sol with new list + new_x = [np.ones(4)] * len(sol.x_sol) + sol.x_sol = new_x + np.testing.assert_array_equal(sol.x_sol[0], np.ones(4)) + + def test_copy(self): + sol = systems.C3Solution(4, 2, 2, 3) + sol2 = copy.copy(sol) + sol3 = copy.deepcopy(sol) + self.assertIsNotNone(sol2) + self.assertIsNotNone(sol3) + + +class TestC3Intermediates(unittest.TestCase): + def test_default_construction(self): + inter = systems.C3Intermediates() + self.assertIsNotNone(inter) + + def test_construction_with_dims(self): + n_x, n_lambda, n_u, N = 4, 2, 2, 3 + inter = systems.C3Intermediates(n_x, n_lambda, n_u, N) + self.assertIsNotNone(inter.z) + self.assertIsNotNone(inter.delta) + self.assertIsNotNone(inter.w) + self.assertIsNotNone(inter.time_vector) + + def test_readwrite_fields(self): + inter = systems.C3Intermediates(4, 2, 2, 3) + new_z = [np.zeros(4)] * len(inter.z) + inter.z = new_z + np.testing.assert_array_equal(inter.z[0], np.zeros(4)) + + def test_copy(self): + inter = systems.C3Intermediates(4, 2, 2, 3) + inter2 = copy.copy(inter) + inter3 = copy.deepcopy(inter) + self.assertIsNotNone(inter2) + self.assertIsNotNone(inter3) + + +class TestC3StatePredictionJoint(unittest.TestCase): + def test_fields(self): + joint = systems.C3StatePredictionJoint() + joint.name = "shoulder" + joint.max_acceleration = 10.0 + self.assertEqual(joint.name, "shoulder") + self.assertAlmostEqual(joint.max_acceleration, 10.0) + + +class TestC3ControllerOptions(unittest.TestCase): + def test_fields(self): + opts = systems.C3ControllerOptions() + opts.solve_time_filter_alpha = 0.5 + opts.publish_frequency = 100.0 + # projection_type is a str per binding + opts.projection_type = "qp" + opts.quaternion_weight = 1.0 + opts.quaternion_regularizer_fraction = 0.1 + self.assertAlmostEqual(opts.solve_time_filter_alpha, 0.5) + self.assertAlmostEqual(opts.publish_frequency, 100.0) + self.assertAlmostEqual(opts.quaternion_weight, 1.0) + self.assertAlmostEqual(opts.quaternion_regularizer_fraction, 0.1) + + def test_nested_c3_options(self): + opts = systems.C3ControllerOptions() + c3_opts = c3.C3Options() + c3_opts.admm_iter = 5 + opts.c3_options = c3_opts + self.assertEqual(opts.c3_options.admm_iter, 5) + + @unittest.skipUnless(HAS_MULTIBODY, "multibody module not available") + def test_nested_lcs_factory_options(self): + opts = systems.C3ControllerOptions() + lcs_opts = multibody.LCSFactoryOptions() + lcs_opts.dt = 0.01 + opts.lcs_factory_options = lcs_opts + self.assertAlmostEqual(opts.lcs_factory_options.dt, 0.01) + + def test_state_prediction_joints(self): + opts = systems.C3ControllerOptions() + joint = systems.C3StatePredictionJoint() + joint.name = "elbow" + joint.max_acceleration = 5.0 + opts.state_prediction_joints = [joint] + self.assertEqual(len(opts.state_prediction_joints), 1) + self.assertEqual(opts.state_prediction_joints[0].name, "elbow") + + def test_load_c3_controller_options(self): + opts = systems.LoadC3ControllerOptions( + "examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml" + ) + self.assertAlmostEqual(opts.publish_frequency, 100.0) + self.assertAlmostEqual(opts.solve_time_filter_alpha, 0.0) + + +class TestTimestampedVector(unittest.TestCase): + def test_construction(self): + # Drake template classes use underscore suffix: TimestampedVector_[float] + vec = systems.TimestampedVector_[float](4) + self.assertIsNotNone(vec) + + def test_set_get_timestamp(self): + vec = systems.TimestampedVector_[float](4) + vec.set_timestamp(1.23) + self.assertAlmostEqual(vec.get_timestamp(), 1.23) + + def test_set_get_data(self): + vec = systems.TimestampedVector_[float](4) + data = np.ones(4) * 2.0 + vec.SetDataVector(data) + np.testing.assert_array_almost_equal(vec.get_data(), data) + + +class TestLCSSimulator(unittest.TestCase): + def _make_lcs(self): + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + return c3.LCS( + np.eye(n_x), + np.zeros((n_x, n_u)), + np.zeros((n_x, n_lambda)), + np.zeros(n_x), + np.zeros((n_lambda, n_x)), + np.eye(n_lambda), + np.zeros((n_lambda, n_u)), + np.ones(n_lambda), + N, + dt, + ) + + def test_construct_from_lcs(self): + lcs = self._make_lcs() + sim = systems.LCSSimulator(lcs) + self.assertIsNotNone(sim) + + def test_construct_from_dims(self): + sim = systems.LCSSimulator(4, 2, 2, 3, 0.01) + self.assertIsNotNone(sim) + + def test_ports(self): + sim = systems.LCSSimulator(4, 2, 2, 3, 0.01) + self.assertIsNotNone(sim.get_input_port_state()) + self.assertIsNotNone(sim.get_input_port_action()) + self.assertIsNotNone(sim.get_input_port_lcs()) + self.assertIsNotNone(sim.get_output_port_next_state()) + + +class TestLCSFactorySystemCallable(unittest.TestCase): + """Verify LCSFactorySystem is importable and constructor signature is correct.""" + + def test_class_exists(self): + self.assertTrue(hasattr(systems, "LCSFactorySystem")) + + def test_constructor_requires_plant(self): + # Should raise TypeError (missing args) not ImportError + with self.assertRaises((TypeError, Exception)): + systems.LCSFactorySystem() + + +class TestC3ControllerCallable(unittest.TestCase): + """Verify C3Controller is importable and constructor signature is correct.""" + + def test_class_exists(self): + self.assertTrue(hasattr(systems, "C3Controller")) + + def test_constructor_requires_plant(self): + with self.assertRaises((TypeError, Exception)): + systems.C3Controller() + + +class TestValueInstantiations(unittest.TestCase): + """Verify Value instantiations are registered.""" + + def test_lcs_value(self): + from pydrake.common.value import Value + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = c3.LCS( + np.eye(n_x), np.zeros((n_x, n_u)), np.zeros((n_x, n_lambda)), + np.zeros(n_x), np.zeros((n_lambda, n_x)), np.eye(n_lambda), + np.zeros((n_lambda, n_u)), np.ones(n_lambda), N, dt, + ) + val = Value[c3.LCS](lcs) + self.assertIsNotNone(val) + + def test_c3solution_value(self): + from pydrake.common.value import Value + sol = systems.C3Solution(4, 2, 2, 3) + val = Value[systems.C3Solution](sol) + self.assertIsNotNone(val) + + def test_c3intermediates_value(self): + from pydrake.common.value import Value + inter = systems.C3Intermediates(4, 2, 2, 3) + val = Value[systems.C3Intermediates](inter) + self.assertIsNotNone(val) + + +if __name__ == "__main__": + unittest.main() diff --git a/bindings/pyc3/test_c3_smoke.py b/bindings/pyc3/test_c3_smoke.py new file mode 100644 index 0000000..01bf321 --- /dev/null +++ b/bindings/pyc3/test_c3_smoke.py @@ -0,0 +1,285 @@ +"""Smoke test for all pyc3 bindings.""" + +import copy +import os +import tempfile + +import numpy as np +import c3 + + +def make_lcs(n_x=4, n_u=2, n_lambda=2, N=3, dt=0.01): + A = np.eye(n_x) + B = np.zeros((n_x, n_u)) + D = np.zeros((n_x, n_lambda)) + d = np.zeros(n_x) + E = np.zeros((n_lambda, n_x)) + F = np.eye(n_lambda) + H = np.zeros((n_lambda, n_u)) + c_vec = np.ones(n_lambda) + return c3.LCS(A, B, D, d, E, F, H, c_vec, N, dt) + + +def test_lcs_simulate_config(): + cfg = c3.LCSSimulateConfig() + cfg.regularized = False + cfg.piv_tol = 1e-8 + cfg.zero_tol = 1e-10 + cfg.min_exp = -4 + cfg.step_exp = 1 + cfg.max_exp = 4 + print("LCSSimulateConfig OK") + + +def test_lcs(): + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = make_lcs(n_x, n_u, n_lambda, N, dt) + + assert lcs.num_states() == n_x + assert lcs.num_inputs() == n_u + assert lcs.num_lambdas() == n_lambda + assert lcs.N() == N + assert lcs.dt() == dt + + # Accessors + _ = lcs.A(), lcs.B(), lcs.D(), lcs.d() + _ = lcs.E(), lcs.F(), lcs.H(), lcs.c() + + # Setters (expect lists of N matrices/vectors) + lcs.set_A([np.eye(n_x)] * N) + lcs.set_B([np.zeros((n_x, n_u))] * N) + lcs.set_D([np.zeros((n_x, n_lambda))] * N) + lcs.set_d([np.zeros(n_x)] * N) + lcs.set_E([np.zeros((n_lambda, n_x))] * N) + lcs.set_F([np.eye(n_lambda)] * N) + lcs.set_H([np.zeros((n_lambda, n_u))] * N) + lcs.set_c([np.ones(n_lambda)] * N) + + # Simulate + x0 = np.zeros(n_x) + u = np.zeros(n_u) + x_next = lcs.Simulate(x0, u) + assert x_next.shape == (n_x,) + + x_next_cfg = lcs.Simulate(x0, u, c3.LCSSimulateConfig()) + assert x_next_cfg.shape == (n_x,) + + # List constructor + A_list = [np.eye(n_x)] * N + B_list = [np.zeros((n_x, n_u))] * N + D_list = [np.zeros((n_x, n_lambda))] * N + d_list = [np.zeros(n_x)] * N + E_list = [np.zeros((n_lambda, n_x))] * N + F_list = [np.eye(n_lambda)] * N + H_list = [np.zeros((n_lambda, n_u))] * N + c_list = [np.ones(n_lambda)] * N + lcs2 = c3.LCS(A_list, B_list, D_list, d_list, E_list, F_list, H_list, c_list, dt) + assert lcs2.num_states() == n_x + + # Copy constructor + lcs3 = c3.LCS(lcs) + assert lcs3.num_states() == n_x + + # __copy__ / __deepcopy__ + lcs4 = copy.copy(lcs) + lcs5 = copy.deepcopy(lcs) + assert lcs4.num_states() == n_x + assert lcs5.num_states() == n_x + + # Placeholder + lcs_ph = c3.LCS.CreatePlaceholderLCS(n_x, n_u, n_lambda, N, dt) + assert lcs_ph.num_states() == n_x + + print("LCS OK") + + +def test_cost_matrices(): + n_x, n_u, n_lambda, N = 4, 2, 2, 3 + Q = [np.eye(n_x)] * (N + 1) + R = [np.eye(n_u)] * N + G = [np.eye(n_lambda)] * N + U = [np.eye(n_u)] * N + + cm = c3.CostMatrices(Q, R, G, U) + assert len(cm.Q) == N + 1 + assert len(cm.R) == N + assert len(cm.G) == N + assert len(cm.U) == N + + cm.Q = Q + cm.R = R + cm.G = G + cm.U = U + + cm_default = c3.CostMatrices() + print("CostMatrices OK") + + +def test_c3_options(): + opts = c3.C3Options() + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = False + opts.num_threads = 1 + opts.delta_option = 0 + opts.M = 100.0 + opts.admm_iter = 10 + opts.gamma = 0.1 + opts.rho_scale = 1.0 + print("C3Options OK") + + +def test_constraint_variable(): + assert c3.ConstraintVariable.STATE is not None + assert c3.ConstraintVariable.INPUT is not None + assert c3.ConstraintVariable.FORCE is not None + print("ConstraintVariable OK") + + +def make_options(n_x=4, n_u=2, n_lambda=2, N=3): + opts = c3.C3Options() + opts.Q = np.eye(n_x) + opts.R = np.eye(n_u) + opts.G = np.eye(n_x + n_u + n_lambda) + opts.U = np.eye(n_x + n_u + n_lambda) + opts.g_vector = [1.0] * n_lambda + opts.u_vector = [1.0] * n_u + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = False + opts.num_threads = 1 + opts.admm_iter = 3 + opts.M = 100.0 + opts.gamma = 0.1 + opts.rho_scale = 1.0 + return opts + + +def test_c3qp(): + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = make_lcs(n_x, n_u, n_lambda, N, dt) + opts = make_options(n_x, n_u, n_lambda, N) + costs = c3.C3.CreateCostMatricesFromC3Options(opts, N) + + x_des = [np.zeros(n_x)] * (N + 1) + solver = c3.C3QP(lcs, costs, x_des, opts) + + x0 = np.zeros(n_x) + solver.Solve(x0) + + _ = solver.GetFullSolution() + _ = solver.GetStateSolution() + _ = solver.GetForceSolution() + _ = solver.GetInputSolution() + _ = solver.GetDualDeltaSolution() + _ = solver.GetDualWSolution() + _ = solver.GetCostMatrices() + _ = solver.GetTargetCost() + _ = solver.GetDynamicConstraints() + _ = solver.GetLinearConstraints() + + solver.UpdateLCS(lcs) + solver.UpdateTarget(x_des) + solver.UpdateCostMatrices(costs) + + solver.AddLinearConstraint( + np.eye(n_x), -np.ones(n_x), np.ones(n_x), c3.ConstraintVariable.STATE + ) + solver.RemoveConstraints() + + solver.AddLinearConstraint( + np.eye(n_u), -np.ones(n_u), np.ones(n_u), c3.ConstraintVariable.INPUT + ) + solver.RemoveConstraints() + + solver.AddLinearConstraint( + np.eye(n_lambda), -np.ones(n_lambda), np.ones(n_lambda), c3.ConstraintVariable.FORCE + ) + solver.RemoveConstraints() + + print("C3QP OK") + + +def test_c3miqp(): + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = make_lcs(n_x, n_u, n_lambda, N, dt) + opts = make_options(n_x, n_u, n_lambda, N) + costs = c3.C3.CreateCostMatricesFromC3Options(opts, N) + x_des = [np.zeros(n_x)] * (N + 1) + solver = c3.C3MIQP(lcs, costs, x_des, opts) + + x0 = np.zeros(n_x) + solver.Solve(x0) + + _ = solver.GetStateSolution() + _ = solver.GetForceSolution() + _ = solver.GetInputSolution() + + print("C3MIQP OK") + + +def test_c3plus(): + n_x, n_u, n_lambda, N, dt = 4, 2, 2, 3, 0.01 + lcs = make_lcs(n_x, n_u, n_lambda, N, dt) + opts = make_options(n_x, n_u, n_lambda, N) + costs = c3.C3.CreateCostMatricesFromC3Options(opts, N) + x_des = [np.zeros(n_x)] * (N + 1) + solver = c3.C3Plus(lcs, costs, x_des, opts) + + x0 = np.zeros(n_x) + solver.Solve(x0) + + _ = solver.GetStateSolution() + _ = solver.GetForceSolution() + _ = solver.GetInputSolution() + + print("C3Plus OK") + + +def test_create_cost_matrices_from_options(): + n_x, n_u, n_lambda, N = 4, 2, 2, 3 + opts = make_options(n_x, n_u, n_lambda, N) + costs = c3.C3.CreateCostMatricesFromC3Options(opts, N) + assert len(costs.Q) == N + 1 + assert len(costs.R) == N + print("CreateCostMatricesFromC3Options OK") + + +def test_load_c3_options(): + """Test LoadC3Options by writing a minimal YAML and loading it.""" + yaml_content = """\ +warm_start: false +scale_lcs: false +end_on_qp_step: false +num_threads: 1 +delta_option: 0 +M: 100.0 +admm_iter: 5 +gamma: 0.1 +rho_scale: 1.0 +""" + with tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False) as f: + f.write(yaml_content) + fname = f.name + try: + opts = c3.LoadC3Options(fname) + assert opts.admm_iter == 5 + assert opts.M == 100.0 + assert abs(opts.gamma - 0.1) < 1e-9 + finally: + os.unlink(fname) + print("LoadC3Options OK") + + +if __name__ == "__main__": + test_lcs_simulate_config() + test_lcs() + test_cost_matrices() + test_c3_options() + test_constraint_variable() + test_c3qp() + test_c3miqp() + test_c3plus() + test_create_cost_matrices_from_options() + test_load_c3_options() + print("\nAll smoke tests passed.") diff --git a/core/c3.cc b/core/c3.cc index f46c20a..a2b243d 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -505,25 +505,37 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A, const VectorXd& lower_bound, const VectorXd& upper_bound, ConstraintVariable constraint) { - if (constraint == 1) { - for (int i = 1; i < N_; ++i) { - user_constraints_.push_back( - prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); - } - } - - if (constraint == 2) { - for (int i = 0; i < N_; ++i) { - user_constraints_.push_back( - prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); - } - } - - if (constraint == 3) { - for (int i = 0; i < N_; ++i) { - user_constraints_.push_back(prog_.AddLinearConstraint( - A, lower_bound, upper_bound, lambda_.at(i))); - } + switch (constraint) { + case ConstraintVariable::STATE: + std::cout << "Adding state constraints" << std::endl; + DRAKE_DEMAND(A.cols() == n_x_); + DRAKE_DEMAND(lower_bound.size() == n_x_); + DRAKE_DEMAND(upper_bound.size() == n_x_); + for (int i = 1; i < N_; ++i) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); + } + break; + case ConstraintVariable::INPUT: + DRAKE_DEMAND(A.cols() == n_u_); + DRAKE_DEMAND(lower_bound.size() == n_u_); + DRAKE_DEMAND(upper_bound.size() == n_u_); + for (int i = 0; i < N_; ++i) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); + } + break; + case ConstraintVariable::FORCE: + DRAKE_DEMAND(A.cols() == n_lambda_); + DRAKE_DEMAND(lower_bound.size() == n_lambda_); + DRAKE_DEMAND(upper_bound.size() == n_lambda_); + for (int i = 0; i < N_; ++i) { + user_constraints_.push_back(prog_.AddLinearConstraint( + A, lower_bound, upper_bound, lambda_.at(i))); + } + break; + default: + throw std::invalid_argument("Invalid constraint variable type."); } } From e0e6161cb9ee1ff1efbb247996d4b9fe1b4dd7b6 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 11:58:12 -0400 Subject: [PATCH 3/9] fix: add python testing dependencies --- .cirrus.yml | 2 ++ .github/workflows/coverage.yml | 5 ++++- bindings/pyc3/c3_multibody_py.cc | 12 ++++++++---- core/c3.cc | 9 ++------- 4 files changed, 16 insertions(+), 12 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index 9f7b764..7e7ccf2 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -18,6 +18,7 @@ jammy_task: --jobs=8 --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... + - apt update && apt install -y python3-pip && python3 -m pip install --user numpy scipy drake - bazel test --local_resources=ram=24000 --local_resources=cpu=8 @@ -48,6 +49,7 @@ noble_task: --jobs=8 --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... + - apt update && apt install -y python3-pip && python3 -m pip install --user numpy scipy drake - bazel test --local_resources=ram=24000 --local_resources=cpu=8 diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 49e887e..9a16c0d 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -37,7 +37,10 @@ jobs: --local_resources=ram=24000 --local_resources=cpu=4 --jobs=4 - //... + --instrumentation_filter="//core[:/],//systems[:/],//multibody[:/]" + //core/... + //systems/... + //multibody/... - name: Report code coverage uses: zgosalvez/github-actions-report-lcov@v4.1.26 with: diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 633f61a..fcee2ae 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -108,13 +108,17 @@ PYBIND11_MODULE(multibody, m) { // Convert enum to the string the C++ struct expects switch (val) { case c3::multibody::ContactModel::kStewartAndTrinkle: - self.contact_model = "stewart_and_trinkle"; break; + self.contact_model = "stewart_and_trinkle"; + break; case c3::multibody::ContactModel::kAnitescu: - self.contact_model = "anitescu"; break; + self.contact_model = "anitescu"; + break; case c3::multibody::ContactModel::kFrictionlessSpring: - self.contact_model = "frictionless_spring"; break; + self.contact_model = "frictionless_spring"; + break; default: - self.contact_model = "unknown"; break; + self.contact_model = "unknown"; + break; } }) .def_readwrite("num_friction_directions", diff --git a/core/c3.cc b/core/c3.cc index a2b243d..428bdb9 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -505,12 +505,11 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A, const VectorXd& lower_bound, const VectorXd& upper_bound, ConstraintVariable constraint) { + DRAKE_DEMAND(lower_bound.size() == A.rows()); + DRAKE_DEMAND(upper_bound.size() == A.rows()); switch (constraint) { case ConstraintVariable::STATE: - std::cout << "Adding state constraints" << std::endl; DRAKE_DEMAND(A.cols() == n_x_); - DRAKE_DEMAND(lower_bound.size() == n_x_); - DRAKE_DEMAND(upper_bound.size() == n_x_); for (int i = 1; i < N_; ++i) { user_constraints_.push_back( prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); @@ -518,8 +517,6 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A, break; case ConstraintVariable::INPUT: DRAKE_DEMAND(A.cols() == n_u_); - DRAKE_DEMAND(lower_bound.size() == n_u_); - DRAKE_DEMAND(upper_bound.size() == n_u_); for (int i = 0; i < N_; ++i) { user_constraints_.push_back( prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); @@ -527,8 +524,6 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A, break; case ConstraintVariable::FORCE: DRAKE_DEMAND(A.cols() == n_lambda_); - DRAKE_DEMAND(lower_bound.size() == n_lambda_); - DRAKE_DEMAND(upper_bound.size() == n_lambda_); for (int i = 0; i < N_; ++i) { user_constraints_.push_back(prog_.AddLinearConstraint( A, lower_bound, upper_bound, lambda_.at(i))); From 017756dbdca883ac05eabb4d9da0f266021b2978 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 12:54:06 -0400 Subject: [PATCH 4/9] ci: add venv for testing python --- .cirrus.yml | 24 ++++++++++++++++++------ .gitignore | 3 ++- bindings/pyc3/requirements.txt | 3 ++- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index 7e7ccf2..aebaf6d 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -12,17 +12,23 @@ jammy_task: test_script: - export CC=clang-15 - export CXX=clang++-15 - - bazel build + - apt update && apt install -y python3-venv + - python3 -m venv .venv --system-site-packages + - source .venv/bin/activate && pip install --quiet -r bindings/pyc3/requirements.txt + - source .venv/bin/activate && bazel build --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - - apt update && apt install -y python3-pip && python3 -m pip install --user numpy scipy drake - - bazel test + - source .venv/bin/activate && source .venv/bin/activate && bazel test --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) --test_output=all --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... @@ -43,18 +49,24 @@ noble_task: test_script: - export CC=clang-15 - export CXX=clang++-15 - - bazel build + - apt update && apt install -y python3-venv + - python3 -m venv .venv --system-site-packages + - source .venv/bin/activate && pip install --quiet -r bindings/pyc3/requirements.txt + - source .venv/bin/activate && bazel build --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - - apt update && apt install -y python3-pip && python3 -m pip install --user numpy scipy drake - - bazel test + - source .venv/bin/activate && bazel test --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 --test_output=all + --noallow_analysis_cache_discard + --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... noble_test_artifacts: diff --git a/.gitignore b/.gitignore index 7246d0a..d5c5072 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,5 @@ MODULE.bazel.lock **/__pycache__/** .vscode *.ps -genhtml/* \ No newline at end of file +genhtml/* +.venv/* \ No newline at end of file diff --git a/bindings/pyc3/requirements.txt b/bindings/pyc3/requirements.txt index 8ac608b..95f2ebc 100644 --- a/bindings/pyc3/requirements.txt +++ b/bindings/pyc3/requirements.txt @@ -1,2 +1,3 @@ -drake +numpy +scipy From c9bc4f5a7c0455c245045a49c92e2dc755d91bfb Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 13:14:47 -0400 Subject: [PATCH 5/9] test: using a single command --- .cirrus.yml | 80 ++++++++++++++++------------------ bindings/pyc3/requirements.txt | 1 + 2 files changed, 39 insertions(+), 42 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index aebaf6d..0c05472 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -9,28 +9,26 @@ jammy_task: format_script: - apt update && apt install -y clang-format - ./tools/scripts/check_format.sh - test_script: - - export CC=clang-15 - - export CXX=clang++-15 - - apt update && apt install -y python3-venv - - python3 -m venv .venv --system-site-packages - - source .venv/bin/activate && pip install --quiet -r bindings/pyc3/requirements.txt - - source .venv/bin/activate && bazel build - --local_resources=ram=24000 - --local_resources=cpu=8 - --jobs=8 - --noallow_analysis_cache_discard - --python_path=$(which python3) - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST + test_script: | + set -e + export CC=clang-15 + export CXX=clang++-15 + apt update && apt install -y python3-venv python3-full + python3 -m venv .venv + .venv/bin/pip install --quiet -r bindings/pyc3/requirements.txt + export PYTHONPATH=$(.venv/bin/python3 -c "import site; print(':'.join(site.getsitepackages()))") + bazel build \ + --local_resources=ram=24000 \ + --local_resources=cpu=8 \ + --jobs=8 \ + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ //... - - source .venv/bin/activate && source .venv/bin/activate && bazel test - --local_resources=ram=24000 - --local_resources=cpu=8 - --jobs=8 - --noallow_analysis_cache_discard - --python_path=$(which python3) - --test_output=all - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST + bazel test \ + --local_resources=ram=24000 \ + --local_resources=cpu=8 \ + --jobs=8 \ + --test_output=all \ + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ //... jammy_test_artifacts: path: "bazel-testlogs/**/test.xml" @@ -46,28 +44,26 @@ noble_task: format_script: - apt update && apt install -y clang-format - ./tools/scripts/check_format.sh - test_script: - - export CC=clang-15 - - export CXX=clang++-15 - - apt update && apt install -y python3-venv - - python3 -m venv .venv --system-site-packages - - source .venv/bin/activate && pip install --quiet -r bindings/pyc3/requirements.txt - - source .venv/bin/activate && bazel build - --local_resources=ram=24000 - --local_resources=cpu=8 - --jobs=8 - --noallow_analysis_cache_discard - --python_path=$(which python3) - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST + test_script: | + set -e + export CC=clang-15 + export CXX=clang++-15 + apt update && apt install -y python3-venv python3-full + python3 -m venv .venv + .venv/bin/pip install --quiet -r bindings/pyc3/requirements.txt + export PYTHONPATH=$(.venv/bin/python3 -c "import site; print(':'.join(site.getsitepackages()))") + bazel build \ + --local_resources=ram=24000 \ + --local_resources=cpu=8 \ + --jobs=8 \ + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ //... - - source .venv/bin/activate && bazel test - --local_resources=ram=24000 - --local_resources=cpu=8 - --jobs=8 - --test_output=all - --noallow_analysis_cache_discard - --python_path=$(which python3) - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST + bazel test \ + --local_resources=ram=24000 \ + --local_resources=cpu=8 \ + --jobs=8 \ + --test_output=all \ + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ //... noble_test_artifacts: path: "bazel-testlogs/**/test.xml" diff --git a/bindings/pyc3/requirements.txt b/bindings/pyc3/requirements.txt index 95f2ebc..6915233 100644 --- a/bindings/pyc3/requirements.txt +++ b/bindings/pyc3/requirements.txt @@ -1,3 +1,4 @@ +drake numpy scipy From 1c71dc72bfc9f8476e49f477a2cbd7ed25480098 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 14:03:51 -0400 Subject: [PATCH 6/9] test --- .cirrus.yml | 80 ++++++++++++++++++++++++++++------------------------- 1 file changed, 42 insertions(+), 38 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index 0c05472..f45c58f 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -9,26 +9,28 @@ jammy_task: format_script: - apt update && apt install -y clang-format - ./tools/scripts/check_format.sh - test_script: | - set -e - export CC=clang-15 - export CXX=clang++-15 - apt update && apt install -y python3-venv python3-full - python3 -m venv .venv - .venv/bin/pip install --quiet -r bindings/pyc3/requirements.txt - export PYTHONPATH=$(.venv/bin/python3 -c "import site; print(':'.join(site.getsitepackages()))") - bazel build \ - --local_resources=ram=24000 \ - --local_resources=cpu=8 \ - --jobs=8 \ - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ + test_script: + - export CC=clang-15 + - export CXX=clang++-15 + - apt update && apt install -y python3-venv + - python3 -m venv .venv --system-site-packages + - . .venv/bin/activate && pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" + - . .venv/bin/activate && bazel build + --local_resources=ram=24000 + --local_resources=cpu=8 + --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - bazel test \ - --local_resources=ram=24000 \ - --local_resources=cpu=8 \ - --jobs=8 \ - --test_output=all \ - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ + - . .venv/bin/activate && bazel test + --local_resources=ram=24000 + --local_resources=cpu=8 + --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) + --test_output=all + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... jammy_test_artifacts: path: "bazel-testlogs/**/test.xml" @@ -44,26 +46,28 @@ noble_task: format_script: - apt update && apt install -y clang-format - ./tools/scripts/check_format.sh - test_script: | - set -e - export CC=clang-15 - export CXX=clang++-15 - apt update && apt install -y python3-venv python3-full - python3 -m venv .venv - .venv/bin/pip install --quiet -r bindings/pyc3/requirements.txt - export PYTHONPATH=$(.venv/bin/python3 -c "import site; print(':'.join(site.getsitepackages()))") - bazel build \ - --local_resources=ram=24000 \ - --local_resources=cpu=8 \ - --jobs=8 \ - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ + test_script: + - export CC=clang-15 + - export CXX=clang++-15 + - apt update && apt install -y python3-venv + - python3 -m venv .venv --system-site-packages + - . .venv/bin/activate && pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" + - . .venv/bin/activate && bazel build + --local_resources=ram=24000 + --local_resources=cpu=8 + --jobs=8 + --noallow_analysis_cache_discard + --python_path=$(which python3) + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - bazel test \ - --local_resources=ram=24000 \ - --local_resources=cpu=8 \ - --jobs=8 \ - --test_output=all \ - --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST \ + - . .venv/bin/activate && bazel test + --local_resources=ram=24000 + --local_resources=cpu=8 + --jobs=8 + --test_output=all + --noallow_analysis_cache_discard + --python_path=$(which python3) + --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... noble_test_artifacts: path: "bazel-testlogs/**/test.xml" From c0fa1135d167c8e0c4174e0e34ad27489ad77e3d Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 18:06:25 -0400 Subject: [PATCH 7/9] fix: omp warning --- core/c3.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/c3.cc b/core/c3.cc index 428bdb9..3fea84b 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -471,7 +471,7 @@ vector C3::SolveProjection(const vector& U, if (options_.num_threads > 0) { omp_set_dynamic(0); // Explicitly disable dynamic teams omp_set_num_threads(options_.num_threads); // Set number of threads - omp_set_nested(0); + omp_set_max_active_levels(1); // Limit to one level of parallelism omp_set_schedule(omp_sched_static, 0); } From d15b7bc66371d2fff318711b741bc9ae75dad08d Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 18:09:59 -0400 Subject: [PATCH 8/9] test --- .cirrus.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index f45c58f..5ead82e 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -14,7 +14,7 @@ jammy_task: - export CXX=clang++-15 - apt update && apt install -y python3-venv - python3 -m venv .venv --system-site-packages - - . .venv/bin/activate && pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" + - . .venv/bin/activate && python3 - m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" - . .venv/bin/activate && bazel build --local_resources=ram=24000 --local_resources=cpu=8 @@ -51,7 +51,7 @@ noble_task: - export CXX=clang++-15 - apt update && apt install -y python3-venv - python3 -m venv .venv --system-site-packages - - . .venv/bin/activate && pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" + - . .venv/bin/activate && python3 -m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" - . .venv/bin/activate && bazel build --local_resources=ram=24000 --local_resources=cpu=8 From da522f287cbd8c33d1d58ed524671b3753e411d3 Mon Sep 17 00:00:00 2001 From: Meow404 Date: Tue, 10 Mar 2026 18:39:35 -0400 Subject: [PATCH 9/9] test --- .cirrus.yml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.cirrus.yml b/.cirrus.yml index 5ead82e..4212088 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -14,8 +14,8 @@ jammy_task: - export CXX=clang++-15 - apt update && apt install -y python3-venv - python3 -m venv .venv --system-site-packages - - . .venv/bin/activate && python3 - m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Noble CI environment $(which python3)" - - . .venv/bin/activate && bazel build + - . .venv/bin/activate && python3 -m pip install -r bindings/pyc3/requirements.txt && echo "🍏 Installed Python dependencies in Jammy CI environment $(which python3)" + - . .venv/bin/activate && export PYTHONPATH=$(python3 -c "import site; print(':'.join(site.getsitepackages()))") && bazel build --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 @@ -23,13 +23,14 @@ jammy_task: --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - - . .venv/bin/activate && bazel test + - . .venv/bin/activate && export PYTHONPATH=$(python3 -c "import site; print(':'.join(site.getsitepackages()))") && bazel test --local_resources=ram=24000 --local_resources=cpu=8 --jobs=8 --noallow_analysis_cache_discard --python_path=$(which python3) --test_output=all + --test_env=PYTHONPATH --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... jammy_test_artifacts: @@ -67,6 +68,7 @@ noble_task: --test_output=all --noallow_analysis_cache_discard --python_path=$(which python3) + --test_env=PYTHONPATH --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... noble_test_artifacts: