diff --git a/.cirrus.yml b/.cirrus.yml index 9f7b7645..d51417db 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -12,16 +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 + - . .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 + --noallow_analysis_cache_discard + --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - - 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 --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... @@ -30,7 +37,7 @@ jammy_task: format: junit noble_task: - only_if: "$CIRRUS_PR != '' || $CIRRUS_BRANCH == 'main'" # This condition ensures the task runs only for PRs or on the main branch. + only_if: "$CIRRUS_PR != '' || $CIRRUS_BRANCH == 'main'" timeout_in: 120m container: image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42 @@ -42,17 +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 + - . .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 && export PYTHONPATH=$(python3 -c "import site; print(':'.join(site.getsitepackages()))") && 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 + - . .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 --test_output=all + --noallow_analysis_cache_discard + --python_path=$(which python3) --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... noble_test_artifacts: diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 49e887e2..6882e9d0 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -37,7 +37,11 @@ jobs: --local_resources=ram=24000 --local_resources=cpu=4 --jobs=4 - //... + --instrument_test_targets=false + --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/.gitignore b/.gitignore index 7246d0a5..d5c50725 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/BUILD.bazel b/bindings/pyc3/BUILD.bazel index b48c7d8a..6aeb3bd1 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 0560d07e..fcee2ae0 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,13 +84,43 @@ 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<>()) .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 703c58f6..706af936 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) + .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 +186,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) @@ -208,6 +241,12 @@ PYBIND11_MODULE(c3, m) { self.u_vector = val; }) .def_readwrite("warm_start", &C3Options::warm_start) + .def_property( + "penalize_input_change", + [](C3Options const& self) { return self.penalize_input_change; }, + [](C3Options& self, const std::optional& val) { + self.penalize_input_change = val; + }) .def_readwrite("scale_lcs", &C3Options::scale_lcs) .def_readwrite("end_on_qp_step", &C3Options::end_on_qp_step) .def_readwrite("num_threads", &C3Options::num_threads) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index ebeb288f..0c53b347 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>(), @@ -92,6 +93,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/bindings/pyc3/requirements.txt b/bindings/pyc3/requirements.txt index 8ac608b8..69152335 100644 --- a/bindings/pyc3/requirements.txt +++ b/bindings/pyc3/requirements.txt @@ -1,2 +1,4 @@ drake +numpy +scipy diff --git a/bindings/pyc3/test/BUILD.bazel b/bindings/pyc3/test/BUILD.bazel new file mode 100644 index 00000000..1815fe07 --- /dev/null +++ b/bindings/pyc3/test/BUILD.bazel @@ -0,0 +1,81 @@ +# -*- 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", + "PYTHONPATH", + ], +) + +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", + "PYTHONPATH", + ], +) + +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", + "PYTHONPATH", + ], +) + +py_test( + name = "test_traj_eval", + srcs = ["test_traj_eval.py"], + deps = [ + "//bindings/pyc3:c3_py", + "//bindings/pyc3:traj_eval_py", + "@rules_python//python/runfiles", + ], + python_version = "PY3", + tags = ["smoke"], + env_inherit = [ + "GUROBI_HOME", + "GRB_LICENSE_FILE", + "PYTHONPATH", + ], +) diff --git a/bindings/pyc3/test/test_c3.py b/bindings/pyc3/test/test_c3.py new file mode 100644 index 00000000..8ddcba7d --- /dev/null +++ b/bindings/pyc3/test/test_c3.py @@ -0,0 +1,393 @@ +"""Smoke tests for c3 core bindings. + +These tests are primarily smoke tests designed to verify that the Python +bindings are working correctly and can be called without errors. They are +not comprehensive functional tests of the underlying C++ optimization algorithms. +The tests use simple synthetic problems and a realistic cartpole example to +ensure basic binding functionality rather than extensively validating solver +performance, mathematical correctness, or advanced optimization features. +""" + +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 + +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 00000000..06046db1 --- /dev/null +++ b/bindings/pyc3/test/test_multibody.py @@ -0,0 +1,152 @@ +"""Smoke tests for c3 multibody bindings. + +These tests are primarily smoke tests designed to verify that the Python +bindings are working correctly and can be called without errors. They are +not comprehensive functional tests of the underlying C++ multibody mechanics +or contact modeling algorithms. The tests focus on ensuring that configuration +classes can be loaded, options can be set, and basic utility functions can +be called through the Python interface rather than validating complex +multibody dynamics or contact physics. +""" + +import unittest +import multibody +import os + + +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 00000000..3ec52d58 --- /dev/null +++ b/bindings/pyc3/test/test_systems.py @@ -0,0 +1,276 @@ +"""Smoke tests for c3 systems bindings. + +These tests are primarily smoke tests designed to verify that the Python +bindings are working correctly and can be called without errors. They are +not comprehensive functional tests of the underlying C++ systems functionality. +The tests focus on ensuring that classes can be instantiated, methods can be +called, and basic data can be accessed through the Python interface rather +than validating complex system behavior or mathematical correctness. +""" + +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 + + +class TestLCSSimulateConfig(unittest.TestCase): + def test_construction(self): + config = c3.LCSSimulateConfig() + self.assertIsNotNone(config) + + def test_fields(self): + config = c3.LCSSimulateConfig() + config.regularized = True + config.piv_tol = 1e-10 + config.zero_tol = 1e-12 + config.min_exp = -5 + config.step_exp = 2 + config.max_exp = 5 + + self.assertTrue(config.regularized) + self.assertAlmostEqual(config.piv_tol, 1e-10) + self.assertAlmostEqual(config.zero_tol, 1e-12) + self.assertEqual(config.min_exp, -5) + self.assertEqual(config.step_exp, 2) + self.assertEqual(config.max_exp, 5) + + +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/test_traj_eval.py b/bindings/pyc3/test/test_traj_eval.py new file mode 100644 index 00000000..5c92352f --- /dev/null +++ b/bindings/pyc3/test/test_traj_eval.py @@ -0,0 +1,334 @@ +"""Smoke tests for c3 trajectory evaluation bindings. + +These tests are primarily smoke tests designed to verify that the Python +bindings are working correctly and can be called without errors. They are +not comprehensive functional tests of the underlying C++ trajectory evaluation +algorithms. The tests use simple synthetic data to ensure basic binding +functionality rather than validating mathematical correctness or performance. +""" + +import unittest +import numpy as np +import c3 +import traj_eval + + +class TestTrajectoryEvaluator(unittest.TestCase): + def setUp(self): + # Create simple test data following the patterns from core_test.cc + self.n_x, self.n_u, self.n_lambda = 4, 2, 2 + self.N = 3 + self.dt = 0.01 + + # Create simple LCS similar to core_test.cc + self.lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)), + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N, + self.dt, + ) + + # Create test trajectories with non-zero values to get positive cost + self.x_traj = [np.ones(self.n_x) for _ in range(self.N + 1)] + self.u_traj = [np.ones(self.n_u) for _ in range(self.N)] + self.x_des = [np.zeros(self.n_x) for _ in range(self.N + 1)] + + # Create cost matrices - ensure they are proper numpy arrays with correct dtype + self.Q_matrix = np.eye(self.n_x, dtype=np.float64) + self.R_matrix = np.eye(self.n_u, dtype=np.float64) + # For list of matrices, ensure each is a separate array + self.Q_matrices = [ + np.eye(self.n_x, dtype=np.float64) for _ in range(self.N + 1) + ] + self.R_matrices = [np.eye(self.n_u, dtype=np.float64) for _ in range(self.N)] + + def test_compute_quadratic_trajectory_cost_basic(self): + # Test with single matrices + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, self.x_des, self.Q_matrix + ) + self.assertGreater(cost, 0) + + # Test with matrix lists + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, self.x_des, self.Q_matrices + ) + self.assertGreater(cost, 0) + + def test_compute_quadratic_trajectory_cost_single_desired(self): + # Test with single desired vector + x_des_single = np.zeros(self.n_x) + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, x_des_single, self.Q_matrix + ) + self.assertGreater(cost, 0) + + # Test with single desired vector and matrix lists + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, x_des_single, self.Q_matrices + ) + self.assertGreater(cost, 0) + + def test_compute_quadratic_trajectory_cost_zero_desired(self): + # Test assuming zero desired + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, self.Q_matrix + ) + self.assertGreater(cost, 0) + + # Test with matrix list + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + self.x_traj, self.Q_matrices + ) + self.assertGreater(cost, 0) + + def test_compute_quadratic_trajectory_cost_with_c3(self): + # Create a C3 solver similar to core_test.cc but with penalize_input_change = False + opts = c3.C3Options() + opts.Q = self.Q_matrix + opts.R = self.R_matrix + opts.G = np.eye(self.n_x + self.n_u + self.n_lambda) + opts.U = np.eye(self.n_x + self.n_u + self.n_lambda) + opts.g_vector = [1.0] * self.n_lambda + [0.0] * self.n_u + opts.u_vector = [1.0] * self.n_lambda + [0.0] * self.n_u + opts.warm_start = False + opts.scale_lcs = False + opts.end_on_qp_step = True + opts.num_threads = 1 + opts.admm_iter = 1 + opts.M = 100.0 + opts.gamma = 1.0 + opts.rho_scale = 1.0 + opts.penalize_input_change = False # This should prevent the constraint failure + + costs = c3.C3.CreateCostMatricesFromC3Options(opts, self.N) + solver = c3.C3QP(self.lcs, costs, self.x_des, opts) + solver.Solve(np.zeros(self.n_x)) + + # Test cost computation from C3 solver + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost(solver) + self.assertGreaterEqual(cost, 0) + + # Test with custom matrices + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + solver, self.Q_matrices, self.R_matrices + ) + self.assertGreaterEqual(cost, 0) + + def test_simulate_pd_control_with_lcs(self): + # Test PD control simulation following the core_test.cc pattern + # Kp and Kd should be n_x length with exactly n_u non-zero entries + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + # Set first n_u entries to be non-zero (positions for Kp, velocities for Kd) + Kp[0] = 10.0 # position control + Kp[1] = 10.0 # position control + Kd[2] = 1.0 # velocity damping + Kd[3] = 1.0 # velocity damping + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, self.u_traj, Kp, Kd, self.lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + for x in x_sim: + self.assertEqual(len(x), self.n_x) + for u in u_sim: + self.assertEqual(len(u), self.n_u) + + def test_simulate_pd_control_with_coarse_fine_lcs(self): + # Create fine LCS with smaller dt, following core_test.cc pattern + fine_lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)) / 2.0, # Scale B matrix for finer time step + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N * 2, # Double the time steps + self.dt / 2, # Half the dt + ) + + # Set up Kp and Kd correctly as in the previous test + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + Kp[0] = 10.0 + Kp[1] = 10.0 + Kd[2] = 1.0 + Kd[3] = 1.0 + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, self.u_traj, Kp, Kd, self.lcs, fine_lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + + def test_simulate_lcs_over_trajectory(self): + config = c3.LCSSimulateConfig() + + # Test with initial state + x_init = np.zeros(self.n_x) + x_sim = traj_eval.TrajectoryEvaluator.SimulateLCSOverTrajectory( + x_init, self.u_traj, self.lcs, config + ) + + self.assertEqual(len(x_sim), self.N + 1) + for x in x_sim: + self.assertEqual(len(x), self.n_x) + + # Test with trajectory plan + x_sim = traj_eval.TrajectoryEvaluator.SimulateLCSOverTrajectory( + self.x_traj, self.u_traj, self.lcs, config + ) + + self.assertEqual(len(x_sim), self.N + 1) + + def test_simulate_lcs_with_coarse_fine(self): + # Create fine LCS + fine_lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)), + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N * 2, + self.dt / 2, + ) + + config = c3.LCSSimulateConfig() + x_init = np.zeros(self.n_x) + + # Test with initial state + x_sim = traj_eval.TrajectoryEvaluator.SimulateLCSOverTrajectory( + x_init, self.u_traj, self.lcs, fine_lcs, config + ) + + self.assertEqual(len(x_sim), self.N + 1) + + # Test with trajectory + x_sim = traj_eval.TrajectoryEvaluator.SimulateLCSOverTrajectory( + self.x_traj, self.u_traj, self.lcs, fine_lcs, config + ) + + self.assertEqual(len(x_sim), self.N + 1) + + def test_zero_order_hold_trajectory(self): + upsample_rate = 2 + + # Test single trajectory + upsampled = traj_eval.TrajectoryEvaluator.ZeroOrderHoldTrajectory( + self.u_traj, upsample_rate + ) + + self.assertEqual(len(upsampled), len(self.u_traj) * upsample_rate) + + # Test state and input trajectories + x_up, u_up = traj_eval.TrajectoryEvaluator.ZeroOrderHoldTrajectories( + self.x_traj, self.u_traj, upsample_rate + ) + + # Following the pattern from core_test.cc: + # x_traj has N+1 elements, u_traj has N elements + # ZeroOrderHoldTrajectories upsamples x_traj[:-1] and adds back the last element + self.assertEqual(len(x_up), (len(self.x_traj) - 1) * upsample_rate + 1) + self.assertEqual(len(u_up), len(self.u_traj) * upsample_rate) + + def test_downsample_trajectory(self): + # Create upsampled trajectory first + upsample_rate = 2 + upsampled_u = traj_eval.TrajectoryEvaluator.ZeroOrderHoldTrajectory( + self.u_traj, upsample_rate + ) + + # Downsample back + downsampled = traj_eval.TrajectoryEvaluator.DownsampleTrajectory( + upsampled_u, upsample_rate + ) + + self.assertEqual(len(downsampled), len(self.u_traj)) + + # Test with trajectories + x_up, u_up = traj_eval.TrajectoryEvaluator.ZeroOrderHoldTrajectories( + self.x_traj, self.u_traj, upsample_rate + ) + + x_down, u_down = traj_eval.TrajectoryEvaluator.DownsampleTrajectories( + x_up, u_up, upsample_rate + ) + + self.assertEqual(len(x_down), len(self.x_traj)) + self.assertEqual(len(u_down), len(self.u_traj)) + + def test_check_coarse_and_fine_lcs_compatibility(self): + # Create compatible fine LCS + fine_lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)), + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N * 2, # Double time steps + self.dt / 2, # Half dt + ) + + upsample_rate = ( + traj_eval.TrajectoryEvaluator.CheckCoarseAndFineLCSCompatibility( + self.lcs, fine_lcs + ) + ) + + self.assertEqual(upsample_rate, 2) + + def test_check_lcs_and_trajectory_compatibility(self): + # Test with valid trajectories - should not raise + traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( + self.lcs, self.x_traj + ) + + # Test with state and input + traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( + self.lcs, self.x_traj, self.u_traj + ) + + # Test with lambda trajectory - lambda should have N elements (not N+1) + lambda_traj = [np.ones(self.n_lambda) for _ in range(self.N)] + traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( + self.lcs, self.x_traj, self.u_traj, lambda_traj + ) + + def test_trajectory_compatibility_errors(self): + # Test with wrong dimensions - should raise + wrong_x = [np.ones(self.n_x + 1) for _ in range(self.N + 1)] + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( + self.lcs, wrong_x + ) + + # Test with wrong length + short_x = [np.ones(self.n_x) for _ in range(self.N)] # Missing one state + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( + self.lcs, short_x + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index 4ca74726..b7df69e6 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -20,101 +20,139 @@ PYBIND11_MODULE(traj_eval, m) { // the Python bindings only expose a finite set of predefined overloads. If // additional input combinations are needed for Python use cases, they must be // explicitly added to this file with corresponding static_cast declarations. - - // LCSSimulateConfig struct bindings - 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); + // + // IMPORTANT: Binding registration order matters for overload resolution. + // pybind11 will try to convert a numpy 2D array to std::vector + // (a vector of row vectors) before trying Eigen::MatrixXd. Therefore, + // single-matrix (Eigen::MatrixXd) overloads MUST be registered before + // vector-of-matrices (std::vector) overloads. // TrajectoryEvaluator class bindings py::class_(m, "TrajectoryEvaluator") - // ComputeQuadraticTrajectoryCost overloads + // --- ComputeQuadraticTrajectoryCost overloads --- + // Single-matrix overloads registered FIRST to prevent pybind11 from + // incorrectly converting a single numpy matrix to vector. + + // Binding: (vector, VectorXd, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const std::vector&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), - "Compute quadratic cost for full trajectory cost computation") + [](const std::vector& data, + const Eigen::VectorXd& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, data_des, cost_matrix); + }, + py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single desired vector and single cost " + "matrix.") + // Binding: (vector, vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const std::vector&, - const Eigen::MatrixXd&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + [](const std::vector& data, + const std::vector& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, data_des, cost_matrix); + }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), - "Compute quadratic cost with single cost matrix") + "Compute quadratic cost with single cost matrix.") + // Binding: (vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const Eigen::VectorXd&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), - "Compute quadratic cost with single desired vector") + [](const std::vector& data, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, cost_matrix); + }, + py::arg("data"), py::arg("cost_matrix"), + "Compute quadratic cost with single matrix and zero desired.") + + // Vector-of-matrices overloads registered AFTER single-matrix overloads. + + // Binding: (vector, VectorXd, vector) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const Eigen::VectorXd&, - const Eigen::MatrixXd&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), - "Compute quadratic cost with single desired vector and single cost " - "matrix") + [](const std::vector& data, + const Eigen::VectorXd& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, data_des, cost_matrices); + }, + py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost with single desired vector.") + // Binding: (vector, vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("data"), py::arg("cost_matrices"), - "Compute quadratic cost assuming zero desired data") + [](const std::vector& data, + const std::vector& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, data_des, cost_matrices); + }, + py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost for full trajectory cost computation.") + // Binding: (vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const Eigen::MatrixXd&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("data"), py::arg("cost_matrix"), - "Compute quadratic cost with single matrix and zero desired") + [](const std::vector& data, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(data, cost_matrices); + }, + py::arg("data"), py::arg("cost_matrices"), + "Compute quadratic cost assuming zero desired data.") + + // C3-based overloads: single-matrix overloads first. + + // Binding: (C3*) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), - py::arg("c3"), "Compute cost from C3 optimizer solution") + [](C3* c3) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(c3); + }, + py::arg("c3"), "Compute cost from C3 optimizer solution.") + // Binding: (C3*, MatrixXd, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + [](C3* c3, const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(c3, Q, R); + }, py::arg("c3"), py::arg("Q"), py::arg("R"), - "Compute cost from C3 with custom cost matrices") + "Compute cost from C3 with single Q and R matrices.") + // Binding: (C3*, MatrixXd, vector) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + [](C3* c3, const Eigen::MatrixXd& Q, + const std::vector& R) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(c3, Q, R); + }, py::arg("c3"), py::arg("Q"), py::arg("R"), - "Compute cost from C3 with single Q matrix") + "Compute cost from C3 with single Q matrix.") + // Binding: (C3*, vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast&, - const Eigen::MatrixXd&)>( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + [](C3* c3, const std::vector& Q, + const Eigen::MatrixXd& R) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(c3, Q, R); + }, py::arg("c3"), py::arg("Q"), py::arg("R"), - "Compute cost from C3 with single R matrix") + "Compute cost from C3 with single R matrix.") + // Binding: (C3*, vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", - static_cast( - &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + [](C3* c3, const std::vector& Q, + const std::vector& R) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(c3, Q, R); + }, py::arg("c3"), py::arg("Q"), py::arg("R"), - "Compute cost from C3 with single Q and R matrices") + "Compute cost from C3 with custom cost matrices.") // SimulatePDControlWithLCS overloads .def_static( diff --git a/core/c3.cc b/core/c3.cc index daa1b555..a7837d63 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -470,7 +470,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_max_active_levels(1); + omp_set_max_active_levels(1); // Limit to one level of parallelism omp_set_schedule(omp_sched_static, 0); } @@ -503,25 +503,32 @@ vector C3::SolveProjection(const vector& U, void C3::AddLinearConstraint(const 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))); - } + DRAKE_DEMAND(lower_bound.size() == A.rows()); + DRAKE_DEMAND(upper_bound.size() == A.rows()); + switch (constraint) { + case ConstraintVariable::STATE: + DRAKE_DEMAND(A.cols() == 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_); + 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_); + 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."); } } diff --git a/examples/python/lcs_factory_example.py b/examples/python/lcs_factory_example.py index 879df74c..168b4178 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 92ee7cad..ecf86948 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, ) )