diff --git a/.bazelrc b/.bazelrc index 5197273..764b3ec 100644 --- a/.bazelrc +++ b/.bazelrc @@ -51,4 +51,4 @@ build --action_env=LD_LIBRARY_PATH= # use python3 by default build --python_path=python3 -build --define=WITH_GUROBI=ON +build --define=WITH_GUROBI=ON \ No newline at end of file diff --git a/.cirrus.yml b/.cirrus.yml index 84b879a..9f7b764 100644 --- a/.cirrus.yml +++ b/.cirrus.yml @@ -1,10 +1,14 @@ registry_config: ENCRYPTED[!88cf0d757d2f8b93dca9e57dc166b65ddedef6378e7ac12a91a022ab3fb28dd47b10d452dc5c53a68e144e6bdbae999b!] jammy_task: + only_if: "$CIRRUS_PR != '' || $CIRRUS_BRANCH == 'main'" # This condition ensures the task runs only for PRs or on the main branch. timeout_in: 120m container: image: ghcr.io/dairlab/docker-dair/jammy-dair-base:v1.42 cpu: 8 memory: 24 + format_script: + - apt update && apt install -y clang-format + - ./tools/scripts/check_format.sh test_script: - export CC=clang-15 - export CXX=clang++-15 @@ -17,19 +21,24 @@ jammy_task: - bazel test --local_resources=ram=24000 --local_resources=cpu=8 + --jobs=8 + --test_output=all --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - always: - jammy_test_artifacts: - path: "bazel-testlogs/**/test.xml" - format: junit + jammy_test_artifacts: + path: "bazel-testlogs/**/test.xml" + 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. timeout_in: 120m container: image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42 cpu: 8 memory: 24 + format_script: + - apt update && apt install -y clang-format + - ./tools/scripts/check_format.sh test_script: - export CC=clang-15 - export CXX=clang++-15 @@ -42,9 +51,10 @@ noble_task: - bazel test --local_resources=ram=24000 --local_resources=cpu=8 + --jobs=8 + --test_output=all --remote_cache=http://$CIRRUS_HTTP_CACHE_HOST //... - always: - noble_test_artifacts: - path: "bazel-testlogs/**/test.xml" - format: junit + noble_test_artifacts: + path: "bazel-testlogs/**/test.xml" + format: junit diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml new file mode 100644 index 0000000..fbc5698 --- /dev/null +++ b/.github/workflows/coverage.yml @@ -0,0 +1,79 @@ +name: C3 Coverage +on: + push: + branches: + - main + pull_request: +jobs: + # Only run coverage on Noble + coverage: + runs-on: ubuntu-latest + concurrency: + group: ci-${{ github.ref }} + cancel-in-progress: true + container: + image: ghcr.io/dairlab/docker-dair/noble-dair-base:v1.42 + credentials: + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + options: --cpus 4 + steps: + - name: Check out repository code + uses: actions/checkout@v4 + - name: Install required dependencies + run: apt update && apt install -y lcov + - name: Restore coverage cache + id: c3-cov-cache-restore + uses: actions/cache/restore@v4 + with: + path: ~/.cache/bazel + key: c3-bazel-cov-cache-${{ hashFiles('**/BUILD.bazel') }} + restore-keys: | + c3-bazel-cov-cache- + - name: Generate Coverage + run: bazel coverage + --combined_report=lcov + --local_resources=ram=24000 + --local_resources=cpu=4 + --jobs=4 + //... + - name: Report code coverage + uses: zgosalvez/github-actions-report-lcov@v4.1.26 + with: + coverage-files: bazel-out/_coverage/_coverage_report.dat + minimum-coverage: 90 + artifact-name: noble-code-coverage-report + github-token: ${{ secrets.GITHUB_TOKEN }} + update-comment: true + - name: Save coverage cache + id: c3-cov-cache-save + if: always() && !cancelled() && steps.c3-cov-cache-restore.outputs.cache-hit != 'true' + uses: actions/cache/save@v4 + with: + key: ${{ steps.c3-cov-cache-restore.outputs.cache-primary-key }} + path: ~/.cache/bazel + - run: echo "🍏 This job's status is ${{ job.status }}." + # Deploy job + deploy: + # Add a dependency to the build job + needs: coverage + if: ${{ github.event_name == 'push' }} + + # Grant GITHUB_TOKEN the permissions required to make a Pages deployment + permissions: + pages: write # to deploy to Pages + id-token: write # to verify the deployment originates from an appropriate source + + # Deploy to the github-pages environment + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + + # Specify runner + deployment step + runs-on: ubuntu-latest + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 # or specific "vX.X.X" version tag for this action + with: + artifact_name: noble-code-coverage-report diff --git a/.gitignore b/.gitignore index b24cae2..7246d0a 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,5 @@ MODULE.bazel.lock **/__pycache__/** .vscode -*.ps \ No newline at end of file +*.ps +genhtml/* \ No newline at end of file diff --git a/BUILD.bazel b/BUILD.bazel index 746710c..a791480 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -1,2 +1,28 @@ -# This is an empty BUILD file, to ensure that the project's root directory is a -# bazel package. +LIBC3_COMPONENTS = [ + "//core:core", + "//multibody:multibody", + "//systems:systems", +] + +package(default_visibility = ["//visibility:public"]) + +# Filegroup collecting all headers +filegroup( + name = "c3_headers", + srcs = [ + "//core:headers", + "//multibody:headers", + "//systems:headers", + ], +) + +# Combined target that provides both the shared library and headers +cc_library( + name = "libc3", + hdrs = [":c3_headers"], # Changed from srcs to hdrs for headers + deps = LIBC3_COMPONENTS + [ + "@drake//:drake_shared_library", + ], + include_prefix = "c3", + visibility = ["//visibility:public"], +) \ No newline at end of file diff --git a/README.md b/README.md index 32deea3..4223d40 100644 --- a/README.md +++ b/README.md @@ -1,28 +1,157 @@ -# C3 -Consensus Complementarity Control +# C3: Consensus Complementarity Control -This is a standalone repository for the C3 algorithm. For more in-depth examples, see dairlib. -Currently we only officially support Ubuntu 22.04. +[**Main Paper: Consensus Complementarity Control**](https://arxiv.org/abs/2304.11259) -## Build from source -1. Clone C3 (Don't change to the c3 directory yet) -```shell -git clone --filter=blob:none git@github.com:DAIRLab/c3.git -``` +

+ + Build (Noble) + Noble Test Report + +    + + Build (Jammy) + Jammy Test + +    + Coverage + C3 Coverage + +

-2. Install Drake's dependencies by running the commands below. This will download the specific Drake release used by C3, and install the corresponding dependencies. -```shell -git clone --depth 1 --branch v1.35.0 https://github.com/RobotLocomotion/drake.git -sudo drake/setup/ubuntu/install_prereqs.sh -``` -3. Follow the instructions at https://drake.mit.edu/bazel.html to install Gurobi 10.0 -4. Change to the C3 Directory, and build the repo: -```shell -cd c3 -bazel build ... +This repository contains the reference implementation of the [Consensus Complementarity Control (C3)](https://arxiv.org/abs/2304.11259) algorithm. For more in-depth examples, see [dairlib](https://github.com/DAIRLab/dairlib). +**Officially supported OS:** Ubuntu 22.04. + +--- + +## Table of Contents + +- [Setup](#setup) +- [Build Instructions](#build-instructions) +- [Testing & Coverage](#testing-and-coverage) +- [Running Examples](#running-examples) +- [Directory Structure](#directory-structure) +- [Reference](#reference) + +--- + +## Setup + +1. **Install Bazel or Bazelisk:** + You can install Bazelisk (a user-friendly launcher for Bazel) or Bazel directly. Bazelisk is recommended as it automatically manages Bazel versions. + + **To install Bazelisk:** + ```sh + sudo apt-get update + sudo apt-get install -y curl + sudo curl -L https://github.com/bazelbuild/bazelisk/releases/latest/download/bazelisk-linux-amd64 -o /usr/local/bin/bazel + sudo chmod +x /usr/local/bin/bazel + ``` + For more details and to find a specific version, visit the [Bazelisk releases page](https://github.com/bazelbuild/bazelisk/releases). Choose a version compatible with your system and project requirements. + + **Or, to install Bazel directly:** + Follow the instructions at [Bazel's official installation guide](https://bazel.build/install/ubuntu). +2. **Clone C3 (do not `cd` into the directory yet):** + ```sh + git clone --filter=blob:none git@github.com:DAIRLab/c3.git + ``` + +2. **Install Drake and its dependencies:** + ```sh + git clone --depth 1 --branch v1.35.0 https://github.com/RobotLocomotion/drake.git + sudo drake/setup/ubuntu/install_prereqs.sh + ``` + +3. **Install Gurobi 10.0:** + Follow the instructions at [Drake's Gurobi setup page](https://drake.mit.edu/bazel.html) to install Gurobi 10.0. + +4. **(Optional) Remove Drake clone:** + You may delete the Drake directory after installing dependencies. + +--- + +## Build Instructions + +1. **Change to the C3 directory:** + ```sh + cd c3 + ``` + +2. **Build the repository using Bazel:** + ```sh + bazel build ... + ``` + +--- + +## Testing and Coverage + +- **Run all unit tests:** + ```sh + bazel test ... --test_output=all + ``` + +- **Run a specific test:** + ```sh + bazel test //systems:systems_test + ``` + +- **Run coverage:** + ```sh + bazel coverage --combined_report=lcov ... + genhtml --branch-coverage --output genhtml "$(bazel info output_path)/_coverage/_coverage_report.dat" # Generates the HTML report to be viewed inside the genhtml folder + ``` + +## Running Examples + +This repository provides several C++ and Python examples demonstrating how to use the C3 library for different systems and workflows. + +--- + +### Quick Start + +- **C++ Examples:** + See the [C3 Standalone Example](./examples/README.md#c3-standalone-example), [C3 Controller Example](./examples/README.md#c3-controller-example), and [LCS Factory System Example](./examples/README.md#lcs-factory-system-example) sections in the examples README for build and run instructions. + +- **Python Examples:** + See the [Python Examples](./examples/README.md#python-examples) section in the examples README for how to build and run the Python scripts using Bazel. + +--- + +For more information, including how to select different problems, visualize results, and understand the system architectures, refer to the [`examples/README.md`](./examples/README.md) file. + +--- + +## Directory Structure + +```plaintext +c3/ +├── bindings/ # Python bindings and tests +├── core/ # Core algorithm implementation +├── examples/ # Example applications and simulations +├── systems/ # Drake systems and tests +├── multibody/ # algorithms for computation in multibody environments +├── third_party/ # External dependencies +└── MODULE.bazel # Bazel module file ``` -5. Run an example program -```shell -bazel-bin/bindings/test/c3_py_test +--- + +## Reference + +For a detailed explanation of the C3 algorithm, please refer to the [main paper](https://arxiv.org/abs/2304.11259). Additional resources and in-depth examples can be found in the [dairlib repository](https://github.com/DAIRLab/dairlib). + +## Citation +If you use C3 in your research, please cite: + +```bibtex +@article{Aydinoglu2024, + title = {Consensus Complementarity Control for Multi-Contact MPC}, + author = {Aydinoglu, Alp and Wei, Adam and Huang, Wei-Cheng and Posa, Michael}, + year = {2024}, + month = jul, + journal = {IEEE Transactions on Robotics (TRO)}, + youtube = {L57Jz3dPwO8}, + arxiv = {2304.11259}, + doi = {10.1109/TRO.2024.3435423}, + url = {https://ieeexplore.ieee.org/document/10614849} +} ``` -6. You may delete the copy of Drake we cloned in step 3 diff --git a/bindings/pyc3/BUILD.bazel b/bindings/pyc3/BUILD.bazel index a1611e6..789b30d 100644 --- a/bindings/pyc3/BUILD.bazel +++ b/bindings/pyc3/BUILD.bazel @@ -35,7 +35,6 @@ pybind_py_library( cc_srcs = ["c3_multibody_py.cc"], py_deps = [ ":module_py", - "@drake//bindings/pydrake" ], py_imports = ["."], ) @@ -53,7 +52,6 @@ pybind_py_library( cc_srcs = ["c3_systems_py.cc"], py_deps = [ ":module_py", - "@drake//bindings/pydrake" ], py_imports = ["."], ) @@ -100,12 +98,16 @@ py_package( py_wheel( name = "pyc3_wheel", + author = "DAIRLab", # Package data. We're building "example_minimal_package-0.0.1-py3-none-any.whl" distribution = "pyc3", license = "MIT", platform = "linux_x86_64", python_tag = "py3", strip_path_prefixes = ["bindings/"], + requires_file = "//bindings/pyc3:requirements.txt", version = "0.0.2", - deps = [":c3_pkg"], + deps = [ + ":c3_pkg", + ], ) diff --git a/bindings/pyc3/README.md b/bindings/pyc3/README.md new file mode 100644 index 0000000..9307fed --- /dev/null +++ b/bindings/pyc3/README.md @@ -0,0 +1,101 @@ +# pyc3 Python Bindings + +This directory contains the Bazel build configuration and source files for the `pyc3` Python bindings for C3 (Consensus Complementarity Control). + +The `pyc3` package provides Python bindings for the C3 algorithm, allowing you to work with Linear Complementarity Systems (LCS), solve Mixed-Integer Quadratic Programs (MIQP), and control systems using the C3 framework. + +--- + +## Prerequisites + +Before building and installing pyc3, ensure you have: + +1. **Drake**: C3 depends on Drake for multibody dynamics and optimization +2. **Gurobi**: Required for the MIQP solver (version 10.0 recommended) +3. **Bazel**: For building the project +4. **Python 3.8+**: With development headers + +For detailed setup instructions, see the main [C3 README](../../README.md). + +--- + +## Building and Installing pyc3 in a Virtual Environment + +Follow these steps to build the `pyc3` Python wheel and install it into a virtual environment. + +### 1. Create and Activate a Python Virtual Environment + +```sh +python3 -m venv venv +source venv/bin/activate +``` + +### 2. Build the pyc3 Python Wheel + +From the root of the C3 repository, build the Python wheel using Bazel: + +```sh +bazel build //bindings/pyc3:pyc3_wheel +``` + +This will create a wheel file in the Bazel output directory. + +### 3. Install the pyc3 Wheel + +Install the built wheel into your virtual environment: + +```sh +pip install bazel-bin/bindings/pyc3/pyc3_wheel.whl +``` + +### 4. Verify Installation + +Test that pyc3 was installed correctly: + +```python +python -c 'import pyc3; print("pyc3 installed successfully!")' +``` + +--- + +## Examples + +Complete examples are available in the `examples/python/` directory: + +- **`c3_example.py`**: Basic C3 with toy example +- **`c3_controller_example.py`**: Real-time controller with Drake integration +- **`lcs_factory_example.py`**: LCS creation from multibody systems + +To run an example: + +```sh +cd examples/python +python c3_example.py +``` + +--- + +## Troubleshooting + +### Common Issues + +1. **Import Error**: If you get import errors, ensure Drake is properly installed and your virtual environment is activated. + +2. **Gurobi License**: Make sure your Gurobi license is properly configured: + ```sh + export GUROBI_HOME=/opt/gurobi1000/linux64 + export GRB_LICENSE_FILE=/path/to/your/gurobi.lic + ``` + +3. **Build Errors**: Ensure all prerequisites are installed and Bazel can find Drake: + ```sh + bazel clean + bazel build //bindings/pyc3:pyc3_wheel + ``` + +### Getting Help + +- Check the main [C3 repository issues](https://github.com/DAIRLab/c3/issues) +- Review the [C3 paper](https://arxiv.org/abs/2304.11259) for algorithmic details +- See [dairlib](https://github.com/DAIRLab/dairlib) for more comprehensive examples + diff --git a/bindings/pyc3/__init__.py b/bindings/pyc3/__init__.py index 08bae3a..323a982 100644 --- a/bindings/pyc3/__init__.py +++ b/bindings/pyc3/__init__.py @@ -1,4 +1,5 @@ # Importing everything in this directory to this package +import pydrake from . import * from .c3 import * from .systems import * diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index 4328943..f17ebc9 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -2,9 +2,9 @@ #include #include "core/lcs.h" -#include "multibody/lcs_factory_options.h" #include "multibody/geom_geom_collider.h" #include "multibody/lcs_factory.h" +#include "multibody/lcs_factory_options.h" #include "multibody/multibody_utils.h" #include "drake/bindings/pydrake/common/sorted_pair_pybind.h" diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index c14d323..55927b9 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -124,17 +124,13 @@ PYBIND11_MODULE(c3, m) { .def(py::init&, const C3Options&>(), py::arg("LCS"), py::arg("costs"), py::arg("x_desired"), - py::arg("options")) - .def("GetWarmStartDelta", &C3MIQP::GetWarmStartDelta) - .def("GetWarmStartBinary", &C3MIQP::GetWarmStartBinary); + py::arg("options")); - py::class_(m, "C3QP") - .def(py::init&, const C3Options&>(), - py::arg("LCS"), py::arg("costs"), py::arg("x_desired"), - py::arg("options")) - .def("GetWarmStartDelta", &C3QP::GetWarmStartDelta) - .def("GetWarmStartBinary", &C3QP::GetWarmStartBinary); + py::class_(m, "C3QP").def( + py::init&, const C3Options&>(), + py::arg("LCS"), py::arg("costs"), py::arg("x_desired"), + py::arg("options")); py::class_(m, "LCS") .def(py::init&, diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index 76749f1..cc12670 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -157,7 +157,7 @@ PYBIND11_MODULE(systems, m) { drake::pydrake::AddValueInstantiation(m); drake::pydrake::AddValueInstantiation(m); drake::pydrake::AddValueInstantiation(m); - + py::class_(m, "C3StatePredictionJoint") .def(py::init<>()) .def_readwrite("name", &C3StatePredictionJoint::name) diff --git a/bindings/pyc3/requirements.txt b/bindings/pyc3/requirements.txt new file mode 100644 index 0000000..8ac608b --- /dev/null +++ b/bindings/pyc3/requirements.txt @@ -0,0 +1,2 @@ +drake + diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 010330c..5b80c66 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -3,6 +3,16 @@ package(default_visibility = ["//visibility:public"]) +cc_library( + name = "core", + visibility = ["//visibility:public"], + deps = [ + ":c3", + ":options", + ":lcs", + ], +) + cc_library( name = "options", hdrs = ["c3_options.h", @@ -12,6 +22,13 @@ cc_library( ], ) +filegroup( + name = "default_solver_options", + srcs = [ + "configs/solver_options_default.yaml", + ], +) + cc_library( name = "c3", srcs = [ @@ -30,11 +47,11 @@ cc_library( "c3_miqp.h", "c3_qp.h", ], + data = [ + ":default_solver_options", + ], copts = ["-fopenmp"], linkopts = ["-fopenmp"], - data = glob([ - "configs/**", - ]), deps = [ ":lcs", ":options", @@ -43,6 +60,7 @@ cc_library( "//tools:with_gurobi": ["@gurobi//:gurobi_cxx"], "//conditions:default": [], }), + includes = ['.'] ) cc_library( @@ -70,8 +88,8 @@ cc_library( ) cc_test( - name = "c3_test", - srcs = ["test/c3_test.cc"], + name = "core_test", + srcs = ["test/core_test.cc"], deps = [ ":c3", ":c3_cartpole_problem", @@ -83,4 +101,11 @@ cc_test( ], ) +filegroup( + name = "headers", + srcs = glob([ + "*.h", + ]), + visibility = ["//visibility:public"], +) diff --git a/core/c3.h b/core/c3.h index 0b1eda5..0247f20 100644 --- a/core/c3.h +++ b/core/c3.h @@ -164,7 +164,8 @@ class C3 { * @param options The C3Options object containing configuration values. * @return CostMatrices The initialized cost matrices. */ - static CostMatrices CreateCostMatricesFromC3Options(const C3Options& options, int N); + static CostMatrices CreateCostMatricesFromC3Options(const C3Options& options, + int N); /** * @brief Get a vector of user defined linear constraints. diff --git a/core/c3_miqp.cc b/core/c3_miqp.cc index bc2fbad..144df7c 100644 --- a/core/c3_miqp.cc +++ b/core/c3_miqp.cc @@ -114,12 +114,4 @@ VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, return delta_kc; } -std::vector C3MIQP::GetWarmStartDelta() const { - return warm_start_delta_[0]; -} - -std::vector C3MIQP::GetWarmStartBinary() const { - return warm_start_binary_[0]; -} - } // namespace c3 diff --git a/core/c3_miqp.h b/core/c3_miqp.h index a587e23..878e2d3 100644 --- a/core/c3_miqp.h +++ b/core/c3_miqp.h @@ -14,7 +14,8 @@ static const double kVariableBounds = 10000; class C3MIQP final : public C3 { public: /// Default constructor for time-varying LCS - C3MIQP(const LCS& LCS, const CostMatrices& costs, const std::vector& xdesired, + C3MIQP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, const C3Options& options); ~C3MIQP() override = default; @@ -26,9 +27,6 @@ class C3MIQP final : public C3 { const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index = -1) override; - std::vector GetWarmStartDelta() const; - std::vector GetWarmStartBinary() const; - const int M_; }; diff --git a/core/c3_miqp_no_gurobi.cc b/core/c3_miqp_no_gurobi.cc index 3c91cb6..7f14750 100644 --- a/core/c3_miqp_no_gurobi.cc +++ b/core/c3_miqp_no_gurobi.cc @@ -26,16 +26,4 @@ VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, "projection method."); } -std::vector C3MIQP::GetWarmStartDelta() const { - throw std::runtime_error( - "The Gurobi bindings were not compiled. You'll need to use a different " - "projection method."); -} - -std::vector C3MIQP::GetWarmStartBinary() const { - throw std::runtime_error( - "The Gurobi bindings were not compiled. You'll need to use a different " - "projection method."); -} - } // namespace c3 diff --git a/core/c3_options.h b/core/c3_options.h index 7e59235..a80e122 100644 --- a/core/c3_options.h +++ b/core/c3_options.h @@ -3,7 +3,7 @@ #include "drake/common/yaml/yaml_io.h" #include "drake/common/yaml/yaml_read_archive.h" -namespace c3{ +namespace c3 { struct C3Options { // Hyperparameters @@ -18,7 +18,7 @@ struct C3Options { int delta_option = 1; // 1 initializes the state value of the delta value with x0 - double M = 1000; // big M value for MIQP + double M = 1000; // big M value for MIQP int admm_iter = 3; // total number of ADMM iterations @@ -144,6 +144,4 @@ inline C3Options LoadC3Options(const std::string& filename) { return options; } - - } // namespace c3 \ No newline at end of file diff --git a/core/c3_qp.cc b/core/c3_qp.cc index b4d98f2..df4b3af 100644 --- a/core/c3_qp.cc +++ b/core/c3_qp.cc @@ -88,12 +88,4 @@ VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, return delta_kc; } -std::vector C3QP::GetWarmStartDelta() const { - return warm_start_delta_[0]; -} - -std::vector C3QP::GetWarmStartBinary() const { - return warm_start_binary_[0]; -} - } // namespace c3 \ No newline at end of file diff --git a/core/c3_qp.h b/core/c3_qp.h index 89d97aa..61865e9 100644 --- a/core/c3_qp.h +++ b/core/c3_qp.h @@ -18,8 +18,8 @@ namespace c3 { class C3QP final : public C3 { public: /// Default constructor for time-varying LCS - C3QP(const LCS& LCS, const CostMatrices& costs, const std::vector& xdesired, - const C3Options& options); + C3QP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, const C3Options& options); ~C3QP() override = default; @@ -30,9 +30,6 @@ class C3QP final : public C3 { const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int admm_iteration, const int& warm_start_index = -1) override; - std::vector GetWarmStartDelta() const; - std::vector GetWarmStartBinary() const; - private: inline static void SetC3QPDefaultOsqpOptions( drake::solvers::SolverOptions* solver_options) { diff --git a/core/lcs.cc b/core/lcs.cc index f860017..2bf74b6 100644 --- a/core/lcs.cc +++ b/core/lcs.cc @@ -80,7 +80,7 @@ LCS LCS::CreatePlaceholderLCS(int n_x, int n_u, int n_lambda, int N, return placeholder; } -std::ostream& operator<<(std::ostream& os, const LCS& m){ +std::ostream& operator<<(std::ostream& os, const LCS& m) { os << "LCS with dimensions: " << m.n_ << " states, " << m.m_ << " forces, " << m.k_ << " inputs, timestep of " << m.dt_ << " and horizon of " << m.N_ << "."; diff --git a/core/lcs.h b/core/lcs.h index 4053801..ba2dd13 100644 --- a/core/lcs.h +++ b/core/lcs.h @@ -50,7 +50,8 @@ class LCS { * * @return The state at the next timestep */ - const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, Eigen::VectorXd& u) const; + const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, + Eigen::VectorXd& u) const; /*! * Accessors dynamics terms diff --git a/core/solver_options_io.h b/core/solver_options_io.h index caf8cab..62b88be 100644 --- a/core/solver_options_io.h +++ b/core/solver_options_io.h @@ -2,11 +2,10 @@ #include "drake/solvers/solver_options.h" - namespace c3 { -using drake::solvers::SolverOptions; using drake::solvers::CommonSolverOption; +using drake::solvers::SolverOptions; /* * Struct containing solver options loaded from a YAML. @@ -35,7 +34,7 @@ struct SolverOptionsFromYaml { std::map double_options; std::map string_options; - template + template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(print_to_console)); a->Visit(DRAKE_NVP(log_file_name)); @@ -61,4 +60,4 @@ struct SolverOptionsFromYaml { } }; -} \ No newline at end of file +} // namespace c3 \ No newline at end of file diff --git a/core/test/c3_test.cc b/core/test/core_test.cc similarity index 91% rename from core/test/c3_test.cc rename to core/test/core_test.cc index 560b521..3c90db7 100644 --- a/core/test/c3_test.cc +++ b/core/test/core_test.cc @@ -5,6 +5,8 @@ #include #include "core/c3_miqp.h" +#include "core/c3_qp.h" +#include "core/lcs.h" #include "core/test/c3_cartpole_problem.hpp" #include "drake/math/discrete_algebraic_riccati_equation.h" @@ -37,6 +39,7 @@ using namespace c3; * | UpdateCostMatrix | DONE | * | Solve | - | * | SetOsqpSolverOptions | - | + * | CreatePlaceholderLCS | DONE | * | # of regression tests | 2 | * * It also has an E2E test for ensuring the "Solve()" function and other @@ -331,37 +334,46 @@ TEST_F(C3CartpoleTest, ZSolStaleTest) { } } -// Test the cartpole example -// This test will take some time to complete ~30s -TEST_F(C3CartpoleTest, End2EndCartpoleTest) { - /// initialize ADMM variables (delta, w) - std::vector delta(N, VectorXd::Zero(n + m + k)); - std::vector w(N, VectorXd::Zero(n + m + k)); +// Test if CreatePlaceholderLCS works as expected +TEST_F(C3CartpoleTest, CreatePlaceholder) { + // Create a placeholder LCS object + LCS placeholder = LCS::CreatePlaceholderLCS(n, k, m, N, dt); + // Ensure the object is created without any issues + ASSERT_TRUE(placeholder.HasSameDimensionsAs(*pSystem)); +} + +template +class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem { + protected: + C3CartpoleTypedTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) { + pOpt = std::make_unique(*pSystem, cost, xdesired, options); + } + std::unique_ptr pOpt; +}; - /// initialize ADMM reset variables (delta, w are reseted to these values) - std::vector delta_reset(N, VectorXd::Zero(n + m + k)); - std::vector w_reset(N, VectorXd::Zero(n + m + k)); +using projection_types = ::testing::Types; +TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types); +// Test the cartpole example +// This test will take some time to complete ~30s +TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) { int timesteps = 1000; // number of timesteps for the simulation /// create state and input arrays - std::vector x(timesteps, VectorXd::Zero(n)); - std::vector input(timesteps, VectorXd::Zero(k)); + std::vector x(timesteps, VectorXd::Zero(this->n)); + std::vector input(timesteps, VectorXd::Zero(this->k)); - x[0] = x0; + x[0] = this->x0; int close_to_zero_counter = 0; for (int i = 0; i < timesteps - 1; i++) { - /// reset delta and w (default option) - delta = delta_reset; - w = w_reset; - /// calculate the input given x[i] - pOpt->Solve(x[i]); - input[i] = pOpt->GetInputSolution()[0]; + this->pOpt->Solve(x[i]); + input[i] = this->pOpt->GetInputSolution()[0]; /// simulate the LCS - x[i + 1] = pSystem->Simulate(x[i], input[i]); + x[i + 1] = this->pSystem->Simulate(x[i], input[i]); if (x[i + 1].isZero(0.1)) { close_to_zero_counter++; if (close_to_zero_counter == 30) break; diff --git a/examples/BUILD.bazel b/examples/BUILD.bazel index 6f37f45..4700687 100644 --- a/examples/BUILD.bazel +++ b/examples/BUILD.bazel @@ -1,7 +1,53 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + cc_binary( name = "c3_example", srcs = ["c3_example.cc"], deps = [ "//core:c3", + "@gflags" ], ) + +filegroup( + name = 'example_data', + srcs = glob(['resources/**']) +) + +cc_binary( + name = "c3_controller_example", + srcs = [ + "c3_controller_example.cc", + "common_systems.hpp" + ], + data = [ + ":example_data" + ], + deps = [ + "//core:c3", + "//core:c3_cartpole_problem", + "//systems:systems", + "//systems:system_utils", + ] +) + +cc_binary( + name = "lcs_factory_system_example", + srcs = [ + "lcs_factory_system_example.cc", + "common_systems.hpp" + ], + data = [ + ":example_data", + ], + deps = [ + "//core:c3", + "//core:c3_cartpole_problem", + "//systems:systems", + "//systems:system_utils", + "@gflags" + ] +) \ No newline at end of file diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..c5cf007 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,86 @@ + +# C3 Standalone Example +This example (c3_example.cc) demonstrates how to set up and solve C3 problems for different systems (cartpole, finger gaiting, and pivoting) using only Eigen and the core C3 classes, where we derive analytical dynamics equations instead of involving Drake's MultibodyPlant. It is useful for understanding the core algorithm and for running C3 on custom LCS models. + +## How to Run the C3 Standalone Example +From the root of the repository, build and run the example with: +```sh +# Build the example +bazel build //examples:c3_example + +# Run the example +bazel run //examples:c3_example +``` +By default, the example will run the finger gaiting problem. You can modify the `example` variable in `c3_example.cc` to select cartpole (`example = 0`), finger gaiting (`example = 1`), or pivoting (`example = 2`). + +# C3 Controller Example + +This example demonstrates the integration of a C3 controller with a cartpole system simulated using a Linear Complementarity System (LCS). The objective is to evaluate the controller's ability to stabilize the cartpole while following a desired state trajectory. Real-time visualization is provided via Meshcat and Drake's SceneGraph. + +**System Architecture:** + +![C3 Controller Test Diagram](resources/diagrams/cartpole_softwalls_c3_controller_test.jpg) + +## How to Run the Cartpole C3 Controller Example + +From the root of the repository, build and run the example with: + +```sh +# Build the example +bazel build //examples:c3_controller_example + +# Run the example +bazel run //examples:c3_controller_example +``` + +--- + +# LCS Factory System Example + +The cartpole with softwalls problem is modeled as a Linear Complementarity System (LCS) generated by the LCS factory. The LCS model provides a piecewise-affine approximation of the nonlinear dynamics, generated using the LCS factory. This LCS forms the basis for running the C3 controller, which stabilizes the cartpole while tracking predefined state trajectories. + +**System Architecture Diagrams:** + +Cartpole with Softwalls | Cube Pivoting +:-------------------------:|:-------------------------: +![Cartpole with Softwalls Diagram](resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg)| ![Cube Pivoting Diagram](resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg) +![Cartpole with Softwalls](resources/cartpole_softwalls/cartpole.png)| ![Cube Pivoting](resources/cube_pivoting/pivoting.png) + +## How to Run the LCS Factory System Examples + +### Build the example +```sh +bazel build //examples:lcs_factory_system_example +``` +### Run the example +#### Cartpole with Softwalls +```sh +bazel run //examples:lcs_factory_system_example -- --experiment_type=cartpole_softwalls +``` + +#### Cube Pivoting +```sh +bazel run //examples:lcs_factory_system_example +``` +# Python Examples +Several Python examples are provided in examples/python/. These use the pyc3 bindings and Drake's Python API. + +## How to Run the Python Examples +From the root of the repository, you can build and run the Python examples as follows: + +C3 Standalone Example (Python) +```sh +bazel run //examples/python:c3_example +``` +C3 Controller Example (Python) +```sh +bazel run //examples/python:c3_controller_example +``` +LCS Factory Example (Python) +```sh +bazel run //examples/python:lcs_factory_example +``` +LCS Factory System Example (Python) +```sh +bazel run //examples/python:lcs_factory_system_example -- [--experiment_type cartpole_softwalls] +``` diff --git a/systems/test/c3_controller_test.cc b/examples/c3_controller_example.cc similarity index 96% rename from systems/test/c3_controller_test.cc rename to examples/c3_controller_example.cc index 404e879..973672f 100644 --- a/systems/test/c3_controller_test.cc +++ b/examples/c3_controller_example.cc @@ -1,11 +1,10 @@ // Includes for core controllers, simulators, and test problems. -#include "systems/c3_controller.h" - #include "core/test/c3_cartpole_problem.hpp" +#include "examples/common_systems.hpp" +#include "systems/c3_controller.h" #include "systems/c3_controller_options.h" #include "systems/common/system_utils.hpp" #include "systems/lcs_simulator.h" -#include "systems/test/test_utils.hpp" // Includes for Drake systems and primitives. #include @@ -45,7 +44,8 @@ std::unique_ptr> AddVisualizer( Parser parser(plant.get(), scene_graph); // Load the Cartpole model from an SDF file. - const std::string file = "systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf"; + const std::string file = + "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf"; parser.AddModels(file); plant->Finalize(); @@ -82,7 +82,8 @@ int DoMain() { builder.AddSystem(*(c3_cartpole_problem.pSystem)); C3ControllerOptions options = drake::yaml::LoadYamlFile( - "systems/test/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml"); + "examples/resources/cartpole_softwalls/" + "c3_controller_cartpole_options.yaml"); // Add a ZeroOrderHold system for state updates. auto state_zero_order_hold = diff --git a/examples/c3_example.cc b/examples/c3_example.cc index ed86bef..226cdca 100644 --- a/examples/c3_example.cc +++ b/examples/c3_example.cc @@ -1,6 +1,8 @@ #include -#include #include +#include + +#include #include "core/c3_miqp.h" @@ -14,6 +16,8 @@ using std::vector; using c3::C3Options; using c3::ConstraintVariable; +DEFINE_bool(verbose, false, "Print verbose output during the example run"); + void init_cartpole(int* n_, int* m_, int* k_, int* N_, vector* A_, vector* B_, vector* D_, vector* d_, vector* E_, @@ -130,7 +134,6 @@ int DoMain(int argc, char* argv[]) { opt.AddLinearConstraint(LinIneq2, lowerbound, upperbound, stateconstraint); } - /// initialize ADMM variables (delta, w) std::vector delta(N, VectorXd::Zero(n + m + k)); std::vector w(N, VectorXd::Zero(n + m + k)); @@ -163,7 +166,6 @@ int DoMain(int argc, char* argv[]) { w = w_reset; } - if (example == 2) { init_pivoting(x[i], &nd, &md, &kd, &Nd, &Ad, &Bd, &Dd, &dd, &Ed, &Fd, &Hd, &cd, &Qd, &Rd, &Gd, &Ud, &x0, &xdesired, &options); @@ -172,14 +174,12 @@ int DoMain(int argc, char* argv[]) { C3MIQP opt(system, cost, xdesired, options); } - auto start = std::chrono::high_resolution_clock::now(); /// calculate the input given x[i] opt.Solve(x[i]); input[i] = opt.GetInputSolution()[0]; - - auto finish = std::chrono::high_resolution_clock::now(); + auto finish = std::chrono::high_resolution_clock::now(); std::chrono::duration elapsed = finish - start; std::cout << "Solve time:" << elapsed.count() << std::endl; total_time = total_time + elapsed.count(); @@ -188,7 +188,10 @@ int DoMain(int argc, char* argv[]) { x[i + 1] = system.Simulate(x[i], input[i]); /// print the state - //std::cout << "state: " << x[i + 1] << std::endl; + if (FLAGS_verbose) { + std::cout << "Step: " << i << ", State: " << x[i + 1].transpose() + << ", Input: " << input[i].transpose() << std::endl; + } } std::cout << "Average time: " << total_time / (timesteps - 1) << std::endl; return 0; @@ -197,6 +200,7 @@ int DoMain(int argc, char* argv[]) { } // namespace c3 int main(int argc, char* argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); return c3::DoMain(argc, argv); } diff --git a/systems/test/test_utils.hpp b/examples/common_systems.hpp similarity index 100% rename from systems/test/test_utils.hpp rename to examples/common_systems.hpp diff --git a/systems/test/lcs_factory_system_test.cc b/examples/lcs_factory_system_example.cc similarity index 97% rename from systems/test/lcs_factory_system_test.cc rename to examples/lcs_factory_system_example.cc index 3dc7057..da4fcda 100644 --- a/systems/test/lcs_factory_system_test.cc +++ b/examples/lcs_factory_system_example.cc @@ -1,6 +1,4 @@ // Includes for core controllers, simulators, and test problems. -#include "systems/lcs_factory_system.h" - #include #include @@ -20,11 +18,12 @@ #include #include "core/test/c3_cartpole_problem.hpp" +#include "examples/common_systems.hpp" #include "systems/c3_controller.h" #include "systems/c3_controller_options.h" #include "systems/common/system_utils.hpp" +#include "systems/lcs_factory_system.h" #include "systems/lcs_simulator.h" -#include "systems/test/test_utils.hpp" #include "drake/multibody/plant/externally_applied_spatial_force.h" #include "drake/systems/rendering/multibody_position_to_geometry_pose.h" @@ -121,7 +120,7 @@ int RunCartpoleTest() { AddMultibodyPlantSceneGraph(&plant_builder, 0.01); Parser parser_for_lcs(&plant_for_lcs, &scene_graph_for_lcs); const std::string file_for_lcs = - "systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf"; + "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf"; parser_for_lcs.AddModels(file_for_lcs); plant_for_lcs.Finalize(); @@ -155,12 +154,13 @@ int RunCartpoleTest() { auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.01); Parser parser(&plant, &scene_graph); const std::string file = - "systems/test/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf"; + "examples/resources/cartpole_softwalls/" + "cartpole_softwalls_no_collision_walls.sdf"; parser.AddModels(file); plant.Finalize(); C3ControllerOptions options = drake::yaml::LoadYamlFile( - "systems/test/resources/cartpole_softwalls/" + "examples/resources/cartpole_softwalls/" "c3_controller_cartpole_options.yaml"); std::unique_ptr> plant_diagram_context = @@ -273,7 +273,7 @@ int RunPivotingTest() { AddMultibodyPlantSceneGraph(&plant_builder, 0.0); Parser parser_for_lcs(&plant_for_lcs, &scene_graph_for_lcs); const std::string file_for_lcs = - "systems/test/resources/cube_pivoting/cube_pivoting.sdf"; + "examples/resources/cube_pivoting/cube_pivoting.sdf"; parser_for_lcs.AddModels(file_for_lcs); plant_for_lcs.Finalize(); @@ -315,14 +315,13 @@ int RunPivotingTest() { DiagramBuilder builder; auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.01); Parser parser(&plant, &scene_graph); - const std::string file = - "systems/test/resources/cube_pivoting/cube_pivoting.sdf"; + const std::string file = "examples/resources/cube_pivoting/cube_pivoting.sdf"; parser.AddModels(file); plant.Finalize(); // Load controller options and cost matrices. C3ControllerOptions options = drake::yaml::LoadYamlFile( - "systems/test/resources/cube_pivoting/" + "examples/resources/cube_pivoting/" "c3_controller_pivoting_options.yaml"); C3::CostMatrices cost = C3::CreateCostMatricesFromC3Options( options.c3_options, options.lcs_factory_options.N); diff --git a/bindings/test/BUILD.bazel b/examples/python/BUILD.bazel similarity index 52% rename from bindings/test/BUILD.bazel rename to examples/python/BUILD.bazel index 5606593..608fea4 100644 --- a/bindings/test/BUILD.bazel +++ b/examples/python/BUILD.bazel @@ -2,8 +2,8 @@ package(default_visibility = ["//visibility:public"]) py_binary( - name = "c3_core_py_test", - srcs = ["c3_core_py_test.py"], + name = "c3_example", + srcs = ["c3_example.py"], deps = [ "//bindings/pyc3:pyc3", ], @@ -13,38 +13,38 @@ py_binary( ) py_binary( - name = "c3_controller_py_test", - srcs = ["c3_controller_py_test.py"], + name = "c3_controller_example", + srcs = ["c3_controller_example.py"], deps = [ "//bindings/pyc3:pyc3", - ":c3_core_py_test", + ":c3_example", ], data = [ - "//systems:test_data" + "//examples:example_data" ], ) py_binary( - name = "lcs_factory_py_test", - srcs = ["lcs_factory_py_test.py"], + name = "lcs_factory_example", + srcs = ["lcs_factory_example.py"], deps = [ "//bindings/pyc3:pyc3", ], data = [ "//core:test_data", - "//systems:test_data", "//multibody:test_data", + "//examples:example_data" ], ) py_binary( - name = "lcs_factory_system_py_test", - srcs = ["lcs_factory_system_py_test.py"], + name = "lcs_factory_system_example", + srcs = ["lcs_factory_system_example.py"], deps = [ "//bindings/pyc3:pyc3", - ":c3_core_py_test", + ":c3_example", ], data = [ - "//systems:test_data" + "//examples:example_data" ], ) \ No newline at end of file diff --git a/bindings/test/c3_controller_py_test.py b/examples/python/c3_controller_example.py similarity index 94% rename from bindings/test/c3_controller_py_test.py rename to examples/python/c3_controller_example.py index 2bb7825..9aee58c 100644 --- a/bindings/test/c3_controller_py_test.py +++ b/examples/python/c3_controller_example.py @@ -5,8 +5,8 @@ LCSSimulator, LoadC3ControllerOptions, ) -from test_utils import C3Solution2Input, Vector2TimestampedVector -from bindings.test.c3_core_py_test import ( +from common_systems import C3Solution2Input, Vector2TimestampedVector +from c3_example import ( make_cartpole_with_soft_walls_dynamics, make_cartpole_costs, ) @@ -34,7 +34,7 @@ def AddVisualizer(builder, scene_graph, state_port, time_step=0.0): parser = Parser(plant, scene_graph) # Load the Cartpole model from an SDF file. - file = "systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf" + file = "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf" parser.AddModels(file) plant.Finalize() @@ -62,7 +62,7 @@ def DoMain(): # Initialize the C3 cartpole problem. options = LoadC3ControllerOptions( - "systems/test/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml" + "examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml" ) cartpole = make_cartpole_with_soft_walls_dynamics(N) costs = make_cartpole_costs(cartpole, options.c3_options, N) diff --git a/bindings/test/c3_core_py_test.py b/examples/python/c3_example.py similarity index 100% rename from bindings/test/c3_core_py_test.py rename to examples/python/c3_example.py diff --git a/bindings/test/test_utils.py b/examples/python/common_systems.py similarity index 100% rename from bindings/test/test_utils.py rename to examples/python/common_systems.py diff --git a/bindings/test/lcs_factory_py_test.py b/examples/python/lcs_factory_example.py similarity index 95% rename from bindings/test/lcs_factory_py_test.py rename to examples/python/lcs_factory_example.py index f615b66..879df74 100644 --- a/bindings/test/lcs_factory_py_test.py +++ b/examples/python/lcs_factory_example.py @@ -22,7 +22,7 @@ def make_cube_pivoting_lcs_plant(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant, scene_graph) - parser.AddModels("systems/test/resources/cube_pivoting/cube_pivoting.sdf") + parser.AddModels("examples/resources/cube_pivoting/cube_pivoting.sdf") plant.Finalize() # Build the plant diagram. @@ -66,7 +66,7 @@ def make_cube_pivoting_lcs_plant(): def main(): - c3_controller_options = LoadC3ControllerOptions("systems/test/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() diff --git a/bindings/test/lcs_factory_system_py_test.py b/examples/python/lcs_factory_system_example.py similarity index 96% rename from bindings/test/lcs_factory_system_py_test.py rename to examples/python/lcs_factory_system_example.py index 787f8cb..92ee7ca 100644 --- a/bindings/test/lcs_factory_system_py_test.py +++ b/examples/python/lcs_factory_system_example.py @@ -27,8 +27,8 @@ ConstraintVariable, ) -from test_utils import C3Solution2Input, Vector2TimestampedVector -from bindings.test.c3_core_py_test import ( +from common_systems import C3Solution2Input, Vector2TimestampedVector +from c3_example import ( make_cartpole_with_soft_walls_dynamics, make_cartpole_costs, ) @@ -89,7 +89,7 @@ def CalcSoftWallSpatialForce(self, context, output): def RunCartpoleTest(): # Initialize the C3 cartpole problem. options = LoadC3ControllerOptions( - "systems/test/resources/cartpole_softwalls/" + "examples/resources/cartpole_softwalls/" "c3_controller_cartpole_options.yaml" ) cartpole = make_cartpole_with_soft_walls_dynamics(options.lcs_factory_options.N) @@ -100,7 +100,7 @@ def RunCartpoleTest(): builder = DiagramBuilder() plant_for_lcs, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) parser = Parser(plant_for_lcs, scene_graph) - file = "systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf" + file = "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf" parser.AddModels(file) plant_for_lcs.Finalize() @@ -138,7 +138,7 @@ def RunCartpoleTest(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) parser = Parser(plant, scene_graph) - file = "systems/test/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf" + file = "examples/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf" parser.AddModels(file) plant.Finalize() @@ -252,7 +252,7 @@ def RunPivotingTest(): builder = DiagramBuilder() plant_for_lcs, scene_graph_for_lcs = AddMultibodyPlantSceneGraph(builder, 0.0) parser_for_lcs = Parser(plant_for_lcs, scene_graph_for_lcs) - file_for_lcs = "systems/test/resources/cube_pivoting/cube_pivoting.sdf" + file_for_lcs = "examples/resources/cube_pivoting/cube_pivoting.sdf" parser_for_lcs.AddModels(file_for_lcs) plant_for_lcs.Finalize() @@ -296,13 +296,13 @@ def RunPivotingTest(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) parser = Parser(plant, scene_graph) - file = "systems/test/resources/cube_pivoting/cube_pivoting.sdf" + file = "examples/resources/cube_pivoting/cube_pivoting.sdf" parser.AddModels(file) plant.Finalize() # Load controller options and cost matrices. options = LoadC3ControllerOptions( - "systems/test/resources/cube_pivoting/c3_controller_pivoting_options.yaml" + "examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml" ) cost = C3.CreateCostMatricesFromC3Options( options.c3_options, options.lcs_factory_options.N diff --git a/systems/test/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml similarity index 100% rename from systems/test/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml rename to examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml diff --git a/systems/test/resources/cartpole_softwalls/cartpole.png b/examples/resources/cartpole_softwalls/cartpole.png similarity index 100% rename from systems/test/resources/cartpole_softwalls/cartpole.png rename to examples/resources/cartpole_softwalls/cartpole.png diff --git a/systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf b/examples/resources/cartpole_softwalls/cartpole_softwalls.sdf similarity index 100% rename from systems/test/resources/cartpole_softwalls/cartpole_softwalls.sdf rename to examples/resources/cartpole_softwalls/cartpole_softwalls.sdf diff --git a/systems/test/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf b/examples/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf similarity index 100% rename from systems/test/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf rename to examples/resources/cartpole_softwalls/cartpole_softwalls_no_collision_walls.sdf diff --git a/systems/test/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml similarity index 100% rename from systems/test/resources/cube_pivoting/c3_controller_pivoting_options.yaml rename to examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml diff --git a/systems/test/resources/cube_pivoting/cube_pivoting.sdf b/examples/resources/cube_pivoting/cube_pivoting.sdf similarity index 100% rename from systems/test/resources/cube_pivoting/cube_pivoting.sdf rename to examples/resources/cube_pivoting/cube_pivoting.sdf diff --git a/systems/test/resources/cube_pivoting/pivoting.png b/examples/resources/cube_pivoting/pivoting.png similarity index 100% rename from systems/test/resources/cube_pivoting/pivoting.png rename to examples/resources/cube_pivoting/pivoting.png diff --git a/systems/test/resources/diagrams/cartpole_softwalls_c3_controller_test.jpg b/examples/resources/diagrams/cartpole_softwalls_c3_controller_test.jpg similarity index 100% rename from systems/test/resources/diagrams/cartpole_softwalls_c3_controller_test.jpg rename to examples/resources/diagrams/cartpole_softwalls_c3_controller_test.jpg diff --git a/systems/test/resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg b/examples/resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg similarity index 100% rename from systems/test/resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg rename to examples/resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg diff --git a/systems/test/resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg b/examples/resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg similarity index 100% rename from systems/test/resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg rename to examples/resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg diff --git a/multibody/BUILD.bazel b/multibody/BUILD.bazel index a2a0772..40203ee 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -3,6 +3,15 @@ package(default_visibility = ["//visibility:public"]) +cc_library( + name = "multibody", + visibility = ["//visibility:public"], + deps = [ + ":lcs_factory", + ":options" + ], +) + cc_library( name = "lcs_factory", srcs = ["lcs_factory.cc", @@ -37,15 +46,22 @@ filegroup( ) cc_test( - name = "lcs_factory_test", - srcs = ["test/lcs_factory_test.cc"], + name = "multibody_test", + srcs = ["test/multibody_test.cc"], data = [ ":test_data", - "//systems:test_data", + "//examples:example_data", ], deps = [ ":lcs_factory", "@gtest//:main", ], +) +filegroup( + name = "headers", + srcs = glob([ + "*.h", + ]), + visibility = ["//visibility:public"], ) diff --git a/multibody/multibody_utils.cc b/multibody/multibody_utils.cc index 6d98a8b..15e5052 100644 --- a/multibody/multibody_utils.cc +++ b/multibody/multibody_utils.cc @@ -62,7 +62,6 @@ VectorX GetInput(const MultibodyPlant& plant, const Context& context) { } } - template void SetContext(const MultibodyPlant& plant, const Eigen::Ref>& state, @@ -109,7 +108,6 @@ void SetInputsIfNew(const MultibodyPlant& plant, } } - template void SetContext(const MultibodyPlant& plant, const Eigen::Ref& state, const Eigen::Ref&, diff --git a/multibody/test/lcs_factory_test.cc b/multibody/test/multibody_test.cc similarity index 92% rename from multibody/test/lcs_factory_test.cc rename to multibody/test/multibody_test.cc index dc9bf6d..d2ff762 100644 --- a/multibody/test/lcs_factory_test.cc +++ b/multibody/test/multibody_test.cc @@ -1,12 +1,13 @@ -#include "multibody/lcs_factory.h" -#include + #include #include "multibody/geom_geom_collider.h" +#include "multibody/lcs_factory.h" #include "multibody/multibody_utils.h" #include "drake/common/sorted_pair.h" +#include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/context.h" @@ -79,13 +80,14 @@ GTEST_TEST(LCSFactoryTest, GetNumContactVariables) { EXPECT_THROW(LCSFactory::GetNumContactVariables(options), std::out_of_range); } -class LCSFactoryPivotingTest : public ::testing::TestWithParam { +class LCSFactoryPivotingTest + : public ::testing::TestWithParam> { protected: void SetUp() override { std::tie(plant, scene_graph) = AddMultibodyPlantSceneGraph(&plant_builder, 0.0); Parser parser(plant, scene_graph); - parser.AddModels("systems/test/resources/cube_pivoting/cube_pivoting.sdf"); + parser.AddModels("examples/resources/cube_pivoting/cube_pivoting.sdf"); plant->Finalize(); // Load controller options from YAML file. @@ -135,8 +137,9 @@ class LCSFactoryPivotingTest : public ::testing::TestWithParam { VectorXd::Zero(plant->num_positions() + plant->num_velocities()); drake::VectorX input = VectorXd::Zero(plant->num_actuators()); - options.contact_model = GetParam(); - contact_model = GetContactModelMap().at(GetParam()); + options.contact_model = std::get<0>(GetParam()); + contact_model = GetContactModelMap().at(options.contact_model); + options.num_friction_directions = std::get<1>(GetParam()); lcs_factory = std::make_unique( *plant, plant_context, *plant_autodiff, *plant_context_autodiff, contact_pairs, options); @@ -256,8 +259,11 @@ TEST_P(LCSFactoryPivotingTest, FixSomeModes) { } INSTANTIATE_TEST_SUITE_P(ContactModelTests, LCSFactoryPivotingTest, - ::testing::Values("frictionless_spring", - "stewart_and_trinkle", "anitescu")); + ::testing::Values(std::tuple("frictionless_spring", 0), + std::tuple("stewart_and_trinkle", 1), + std::tuple("stewart_and_trinkle", 2), + std::tuple("anitescu", 1), + std::tuple("anitescu", 2))); } // namespace test } // namespace multibody diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index a852667..29eb400 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -4,11 +4,25 @@ package(default_visibility = ["//visibility:public"]) cc_library( - name = "vector", + name = "systems", + visibility = ["//visibility:public"], + deps = [ + ":framework", + ":options", + ":c3_controller", + ":lcs_simulator", + ":lcs_factory_system", + ] +) + +cc_library( + name = "framework", srcs = [ + "framework/c3_output.cc", "framework/timestamped_vector.cc", ], hdrs = [ + "framework/c3_output.h", "framework/timestamped_vector.h", ], deps = [ @@ -16,24 +30,16 @@ cc_library( ], ) -cc_library( - name = "systems", +cc_library( name = "c3_controller", srcs = [ "c3_controller.cc", - "lcs_simulator.cc", - "framework/c3_output.cc", - "lcs_factory_system.cc", ], hdrs = [ "c3_controller.h", - "lcs_simulator.h", - "framework/c3_output.h", - "lcs_factory_system.h", - - ], + ], deps = [ - ":vector", ":options", + ":framework", "//core:c3", "//core:options", "//multibody:lcs_factory", @@ -41,6 +47,36 @@ cc_library( ], ) +cc_library( name = "lcs_simulator", + srcs = [ + "lcs_simulator.cc", + ], + hdrs = [ + "lcs_simulator.h", + ], + deps = [ + "//core:lcs", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "lcs_factory_system", + srcs = [ + "lcs_factory_system.cc", + ], + hdrs = [ + "lcs_factory_system.h", + ], + deps = [ + ":framework", + "//core:lcs", + "//core:options", + "//multibody:lcs_factory", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "options", hdrs = [ @@ -53,50 +89,53 @@ cc_library( ] ) -filegroup( - name = 'test_data', - srcs = glob(['test/resources/**']) -) -cc_binary( - name = "c3_controller_test", - srcs = [ - "test/c3_controller_test.cc", - "test/test_utils.hpp" ], - data = [ - ":test_data" + +cc_library( + name = "system_utils", + hdrs = [ + "common/system_utils.hpp", ], deps = [ - "//core:c3", - ":systems", - ":system_utils", - "//core:c3_cartpole_problem", - ] + "@drake//:drake_shared_library", + ], ) - -cc_binary( - name = "lcs_factory_system_test", +cc_test( + name = "systems_test", srcs = [ - "test/lcs_factory_system_test.cc", - "test/test_utils.hpp" ], + "test/systems_test.cc", + ], data = [ - ":test_data", + "//examples:example_data", ], deps = [ - "//core:c3", ":systems", - ":system_utils", "//core:c3_cartpole_problem", - "@gflags", - ] + "@gtest//:main", + ], + env_inherit = [ + "GUROBI_HOME", + "GRB_LICENSE_FILE" + ], ) -cc_library( - name = "system_utils", - hdrs = [ - "common/system_utils.hpp", +cc_test( + name = "framework_test", + srcs = [ + "test/framework_test.cc" ], deps = [ - "@drake//:drake_shared_library", + ":systems", + ":framework", + "@gtest//:main", ], ) + +filegroup( + name = "headers", + srcs = glob([ + "*.h", + "**/*.h", + ]), + visibility = ["//visibility:public"], +) diff --git a/systems/framework/timestamped_vector.h b/systems/framework/timestamped_vector.h index 44c7118..9843e95 100644 --- a/systems/framework/timestamped_vector.h +++ b/systems/framework/timestamped_vector.h @@ -5,10 +5,10 @@ namespace c3 { namespace systems { -/// TimestampedVector wraps a drake::systems::BasicVector along with a timestamp field -/// The primary purpose of this is to pass-through a message (e.g. LCM) -/// timestamp Uses a length N+1 drake::systems::BasicVector to store a vector of length N and a -/// timestamp The timestamp is stored as the final element (Nth) +/// TimestampedVector wraps a drake::systems::BasicVector along with a timestamp +/// field The primary purpose of this is to pass-through a message (e.g. LCM) +/// timestamp Uses a length N+1 drake::systems::BasicVector to store a vector of +/// length N and a timestamp The timestamp is stored as the final element (Nth) template class TimestampedVector : public drake::systems::BasicVector { public: @@ -100,4 +100,4 @@ class TimestampedVector : public drake::systems::BasicVector { }; } // namespace systems -} // namespace dairlib +} // namespace c3 diff --git a/systems/lcs_factory_system.h b/systems/lcs_factory_system.h index 6aed990..15404f5 100644 --- a/systems/lcs_factory_system.h +++ b/systems/lcs_factory_system.h @@ -92,14 +92,6 @@ class LCSFactorySystem : public drake::systems::LeafSystem { return this->get_output_port(lcs_contact_jacobian_port_); } - static LCSFactorySystem* AddToBuilder( - drake::systems::DiagramBuilder& builder, - const drake::multibody::MultibodyPlant& plant_for_lcs, - drake::systems::Context* plant_diagram_context, - const std::vector> - contact_geoms, - LCSFactoryOptions options); - private: /** * @brief Computes the LCS based on the current state and inputs. @@ -136,7 +128,8 @@ class LCSFactorySystem : public drake::systems::LeafSystem { int N_; ///< Number of time steps for the LCS. double dt_; ///< Time step size for the LCS. - std::unique_ptr lcs_factory_; ///< Factory for creating LCS objects. + std::unique_ptr + lcs_factory_; ///< Factory for creating LCS objects. }; } // namespace systems diff --git a/systems/lcs_simulator.cc b/systems/lcs_simulator.cc index eab3b6c..0a65327 100644 --- a/systems/lcs_simulator.cc +++ b/systems/lcs_simulator.cc @@ -15,7 +15,7 @@ LCSSimulator::LCSSimulator(int n_x, int n_u, int n_lambda, int N, double dt) { } // Constructor using an existing LCS object -LCSSimulator::LCSSimulator(const LCS &lcs) { +LCSSimulator::LCSSimulator(const LCS& lcs) { init(lcs.num_states(), lcs.num_inputs(), lcs.num_lambdas(), lcs.N(), lcs.dt()); } @@ -49,7 +49,8 @@ void LCSSimulator::SimulateOneStep( if (!get_input_port_lcs().HasValue(context)) { throw std::runtime_error("Input port LCS [LCSSimulator] not connected"); } - const auto& lcs = this->EvalAbstractInput(context, lcs_input_port_)->get_value(); + const auto& lcs = + this->EvalAbstractInput(context, lcs_input_port_)->get_value(); // Retrieve the current state and action from the input ports auto state = this->EvalVectorInput(context, state_input_port_)->value(); diff --git a/systems/test/README.md b/systems/test/README.md deleted file mode 100644 index 857a3d2..0000000 --- a/systems/test/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# C3 Controller Test - -This test demonstrates the integration of a C3 controller with a cartpole system simulated using a Linear Complementarity System (LCS). The goal is to evaluate the controller's ability to stabilize the cartpole system while adhering to the desired state trajectory. The test includes visualization using Meshcat and Drake's SceneGraph, enabling real-time observation of the system's behavior. - -The diagram below illustrates the system architecture used in this test: - -![C3 Controller Test Diagram](resources/diagrams/cartpole_softwalls_c3_controller_test.jpg) - -# LCS Factory System Test - -The interactions in the cartpole with softwall problem is modeled using as a Linear Complementarity System (LCS) generated by the LCS factory. The LCS serves as the foundation for running the C3 controller, which is tasked with stabilizing the cartpole system while adhering to predefined state trajectories. - -For a detailed view of the system architecture, refer to the diagram below: - -Cartpole with Softwalls | Cube Pivoting -:-------------------------:|:-------------------------: -![Cartpole with Softwalls Diagram](resources/diagrams/cartpole_softwalls_lcs_factory_systems_test.jpg)| ![Cube Pivoting Diagram](resources/diagrams/cube_pivoting_lcs_factory_systems_test.jpg) -![Cartpole with Softwalls](resources/cartpole_softwalls/cartpole.png)| ![Cube Pivoting](resources/cube_pivoting/pivoting.png) \ No newline at end of file diff --git a/systems/test/framework_test.cc b/systems/test/framework_test.cc new file mode 100644 index 0000000..99cffd2 --- /dev/null +++ b/systems/test/framework_test.cc @@ -0,0 +1,86 @@ +#include + +#include "systems/framework/c3_output.h" +#include "systems/framework/timestamped_vector.h" + +namespace c3 { +namespace systems { +namespace test { + +// Test fixture for framework-related tests. +class FrameworkTest : public ::testing::Test { + protected: + void SetUp() override {} +}; + +// Tests construction and data access for TimestampedVector. +TEST_F(FrameworkTest, TimestampedVectorConstructor) { + // Construct from initializer list. + TimestampedVector vec{1.0, 2.0, 3.0}; + EXPECT_EQ(vec.data_size(), 3); + Eigen::VectorXd expected(3); + expected << 1.0, 2.0, 3.0; + EXPECT_TRUE(vec.get_data().isApprox(expected)); + + // Construct from Eigen vector. + TimestampedVector vec2(expected); + EXPECT_TRUE(vec2.get_data().isApprox(expected)); +} + +// Tests data and timestamp accessors/mutators for TimestampedVector. +TEST_F(FrameworkTest, TimestampedVectorAccess) { + // Construct with size. + TimestampedVector vec(3); + EXPECT_EQ(vec.data_size(), 3); + + // Set and get data. + Eigen::VectorXd data(3); + data << 1.0, 2.0, 3.0; + vec.SetDataVector(data); + EXPECT_TRUE(vec.get_data().isApprox(data)); + + // Set and get timestamp. + vec.set_timestamp(42.0); + EXPECT_DOUBLE_EQ(vec.get_timestamp(), 42.0); + + // Copy data vector without timestamp. + Eigen::VectorXd no_ts = vec.CopyVectorNoTimestamp(); + EXPECT_TRUE(no_ts.isApprox(data)); + + // Clone and verify deep copy. + auto clone = vec.Clone(); + EXPECT_TRUE(clone->get_data().isApprox(data)); + EXPECT_DOUBLE_EQ(clone->get_timestamp(), 42.0); +} + +// Tests construction and shape of C3Solution and C3Intermediates. +TEST_F(FrameworkTest, C3SolutionAndIntermediatesConstruction) { + int n_x = 2, n_lambda = 3, n_u = 1, N = 4; + C3Output::C3Solution sol(n_x, n_lambda, n_u, N); + C3Output::C3Intermediates interm(n_x, n_lambda, n_u, N); + + // Verify solution matrix shapes. + EXPECT_EQ(sol.x_sol_.rows(), n_x); + EXPECT_EQ(sol.x_sol_.cols(), N); + EXPECT_EQ(sol.lambda_sol_.rows(), n_lambda); + EXPECT_EQ(sol.lambda_sol_.cols(), N); + EXPECT_EQ(sol.u_sol_.rows(), n_u); + EXPECT_EQ(sol.u_sol_.cols(), N); + EXPECT_EQ(sol.time_vector_.size(), N); + + // Verify intermediates matrix shapes. + EXPECT_EQ(interm.z_.rows(), n_x + n_lambda + n_u); + EXPECT_EQ(interm.z_.cols(), N); + EXPECT_EQ(interm.delta_.rows(), n_x + n_lambda + n_u); + EXPECT_EQ(interm.delta_.cols(), N); + EXPECT_EQ(interm.w_.rows(), n_x + n_lambda + n_u); + EXPECT_EQ(interm.w_.cols(), N); + EXPECT_EQ(interm.time_vector_.size(), N); + + // Construct C3Output from solution and intermediates. + C3Output output(sol, interm); +} + +} // namespace test +} // namespace systems +} // namespace c3 diff --git a/systems/test/systems_test.cc b/systems/test/systems_test.cc new file mode 100644 index 0000000..140225a --- /dev/null +++ b/systems/test/systems_test.cc @@ -0,0 +1,344 @@ +// systems_test.cc +// Unit tests for LCS-based systems and controllers in the c3 project. + +#include +#include +#include + +#include "core/test/c3_cartpole_problem.hpp" +#include "multibody/lcs_factory.h" +#include "systems/c3_controller.h" +#include "systems/c3_controller_options.h" +#include "systems/framework/system_output.h" +#include "systems/lcs_factory_system.h" +#include "systems/lcs_simulator.h" + +using c3::multibody::LCSFactory; +using c3::systems::C3Controller; +using c3::systems::C3ControllerOptions; +using c3::systems::C3Output; +using c3::systems::C3StatePredictionJoint; +using c3::systems::LCSSimulator; +using drake::SortedPair; +using drake::geometry::GeometryId; +using drake::geometry::SceneGraph; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +using drake::systems::Context; +using drake::systems::DiagramBuilder; +using drake::systems::DiscreteValues; +using drake::systems::SystemOutput; +using Eigen::VectorX; + +namespace c3 { +namespace systems { +namespace test { + +// Test fixture for LCSSimulator +class LCSSimulatorTest : public ::testing::Test, public C3CartpoleProblem { + protected: + LCSSimulatorTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81) {} + + void SetUp() override { + context_ = simulator_.CreateDefaultContext(); + output_ = simulator_.AllocateOutput(); + + // Set up dummy inputs for LCS, state, and action + simulator_.get_input_port(0).FixValue(context_.get(), *pSystem); + simulator_.get_input_port(1).FixValue(context_.get(), x0); + VectorX ones = VectorX::Ones(pSystem->num_inputs()); + simulator_.get_input_port(2).FixValue(context_.get(), ones); + } + + LCSSimulator simulator_{*pSystem}; + std::unique_ptr> context_; + std::unique_ptr> output_; +}; + +TEST_F(LCSSimulatorTest, Initialization) { + // Check if the simulator is initialized correctly. + EXPECT_EQ(simulator_.get_input_port_state().size(), pSystem->num_states()); + EXPECT_EQ(simulator_.get_input_port_action().size(), pSystem->num_inputs()); + EXPECT_EQ(simulator_.get_output_port_next_state().size(), + pSystem->num_states()); + + // Test alternate constructor + LCSSimulator simulator(pSystem->num_states(), pSystem->num_inputs(), + pSystem->num_lambdas(), pSystem->N(), pSystem->dt()); + EXPECT_EQ(simulator.get_input_port_state().size(), pSystem->num_states()); + EXPECT_EQ(simulator.get_input_port_action().size(), pSystem->num_inputs()); + EXPECT_EQ(simulator.get_output_port_next_state().size(), + pSystem->num_states()); +} + +TEST_F(LCSSimulatorTest, SimulateOneStep) { + // Simulate one step and check the output. + simulator_.CalcOutput(*context_, output_.get()); + const auto& next_state = output_->get_vector_data(0)->get_value(); + EXPECT_EQ(next_state.size(), pSystem->num_states()); +} + +TEST_F(LCSSimulatorTest, MissingLCS) { + // Should throw if LCS input is missing + auto new_context = simulator_.CreateDefaultContext(); + simulator_.get_input_port(1).FixValue(new_context.get(), x0); + VectorX ones = VectorX::Ones(pSystem->num_inputs()); + simulator_.get_input_port(2).FixValue(new_context.get(), ones); + + EXPECT_THROW(simulator_.CalcOutput(*new_context, output_.get()), + std::runtime_error); +} + +// Test fixture for C3Controller +class C3ControllerTest : public ::testing::Test, public C3CartpoleProblem { + protected: + C3ControllerTest() + : C3CartpoleProblem(0.411, 0.978, 0.6, 0.4267, 0.35, -0.35, 100, 9.81, 10, + 0.1) {} + + void SetUp() override { + // Load controller options from YAML + C3ControllerOptions controller_options = + drake::yaml::LoadYamlFile( + "examples/resources/cartpole_softwalls/" + "c3_controller_cartpole_options.yaml"); + controller_options.publish_frequency = 0; // Forced Update + + // Add prediction for the slider joint + controller_options.state_prediction_joints.push_back( + {.name = "CartSlider", .max_acceleration = 10.0}); + controller_options.lcs_factory_options.N = 10; + controller_options.lcs_factory_options.dt = 0.1; + + // Build plant and scene graph + std::tie(plant, scene_graph) = + AddMultibodyPlantSceneGraph(&plant_builder_, 0.0); + Parser parser(plant, scene_graph); + parser.AddModels( + "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf"); + plant->Finalize(); + + controller_ = + std::make_unique(*plant, cost, controller_options); + + context_ = controller_->CreateDefaultContext(); + output_ = controller_->AllocateOutput(); + discrete_values_ = controller_->AllocateDiscreteVariables(); + + // Set up dummy inputs: state, LCS, and target + auto state_vec = + c3::systems::TimestampedVector(pSystem->num_states()); + state_vec.SetDataVector(x0); + state_vec.set_timestamp(0.0); + controller_->get_input_port_lcs_state().FixValue(context_.get(), state_vec); + controller_->get_input_port_lcs().FixValue(context_.get(), *pSystem); + controller_->get_input_port_target().FixValue(context_.get(), + xdesired.at(0)); + } + + MultibodyPlant* plant{}; + SceneGraph* scene_graph{}; + DiagramBuilder plant_builder_; + std::unique_ptr controller_; + std::unique_ptr> context_; + std::unique_ptr> output_; + std::unique_ptr> discrete_values_; +}; + +TEST_F(C3ControllerTest, CheckInputOutputPorts) { + // Check input and output port sizes and existence + EXPECT_NO_THROW(controller_->get_input_port_lcs_state()); + EXPECT_EQ(controller_->get_input_port_lcs_state().size(), + pSystem->num_states() + 1); // +1 for timestamp + EXPECT_NO_THROW(controller_->get_input_port_lcs()); + EXPECT_EQ(controller_->get_input_port_target().size(), pSystem->num_states()); + EXPECT_NO_THROW(controller_->get_output_port_c3_solution()); + EXPECT_NO_THROW(controller_->get_output_port_c3_intermediates()); +} + +TEST_F(C3ControllerTest, CheckPlannedTrajectory) { + // Should not throw when computing plan with valid inputs + EXPECT_NO_THROW({ + controller_->CalcForcedDiscreteVariableUpdate(*context_, + discrete_values_.get()); + controller_->CalcOutput(*context_, output_.get()); + }); + + // Check C3 solution output + const auto& c3_solution = + output_->get_data(0)->get_value(); + EXPECT_EQ(c3_solution.time_vector_.size(), pSystem->N()); + EXPECT_EQ(c3_solution.x_sol_.rows(), pSystem->num_states()); + EXPECT_EQ(c3_solution.lambda_sol_.rows(), pSystem->num_lambdas()); + EXPECT_EQ(c3_solution.u_sol_.rows(), pSystem->num_inputs()); + EXPECT_EQ(c3_solution.x_sol_.cols(), pSystem->N()); + EXPECT_EQ(c3_solution.lambda_sol_.cols(), pSystem->N()); + EXPECT_EQ(c3_solution.u_sol_.cols(), pSystem->N()); + + // Check C3 intermediates output + const auto& c3_intermediates = + output_->get_data(1)->get_value(); + EXPECT_EQ(c3_intermediates.time_vector_.size(), pSystem->N()); + int total_vars = + pSystem->num_states() + pSystem->num_lambdas() + pSystem->num_inputs(); + EXPECT_EQ(c3_intermediates.z_.rows(), total_vars); + EXPECT_EQ(c3_intermediates.delta_.rows(), total_vars); + EXPECT_EQ(c3_intermediates.w_.rows(), total_vars); + EXPECT_EQ(c3_intermediates.z_.cols(), pSystem->N()); + EXPECT_EQ(c3_intermediates.delta_.cols(), pSystem->N()); + EXPECT_EQ(c3_intermediates.w_.cols(), pSystem->N()); +} + +TEST_F(C3ControllerTest, ThrowsOnMissingLCS) { + // Remove LCS input and expect a runtime error + auto new_context = controller_->CreateDefaultContext(); + auto state_vec = c3::systems::TimestampedVector( + plant->num_positions() + plant->num_velocities()); + state_vec.SetDataVector(x0); + state_vec.set_timestamp(0.0); + controller_->get_input_port_lcs_state().FixValue(new_context.get(), + state_vec); + controller_->get_input_port_target().FixValue(new_context.get(), x0); + + EXPECT_THROW(controller_->CalcForcedDiscreteVariableUpdate( + *new_context, discrete_values_.get()), + std::runtime_error); +} + +TEST_F(C3ControllerTest, TestJointPrediction) { + // Test that joint prediction modifies the first state in the planned + // trajectory + double eps = 1e-8; + controller_->CalcForcedDiscreteVariableUpdate(*context_, + discrete_values_.get()); + controller_->CalcOutput(*context_, output_.get()); + const auto& pre_solution = + output_->get_data(0)->get_value(); + Eigen::VectorXd first_state = pre_solution.x_sol_.col(0).cast(); + EXPECT_TRUE(first_state.isApprox(x0, eps)); + + // Update context with predicted state and check that it changes + context_->SetDiscreteState(*discrete_values_); + EXPECT_NO_THROW(controller_->CalcForcedDiscreteVariableUpdate( + *context_, discrete_values_.get())); + controller_->CalcOutput(*context_, output_.get()); + const auto& post_solution = + output_->get_data(0)->get_value(); + Eigen::VectorXd predicted_first_state = + post_solution.x_sol_.col(0).cast(); + EXPECT_FALSE(predicted_first_state.isApprox(x0, eps)); +} + +// Test fixture for LCSFactorySystem +class LCSFactorySystemTest : public ::testing::Test, public C3CartpoleProblem { + protected: + void SetUp() override { + // Build plant and scene graph + std::tie(plant, scene_graph) = AddMultibodyPlantSceneGraph(&builder, 0.0); + Parser parser(plant, scene_graph); + parser.AddModels( + "examples/resources/cartpole_softwalls/cartpole_softwalls.sdf"); + plant->Finalize(); + + plant_ad = drake::systems::System::ToAutoDiffXd(*plant); + diagram = builder.Build(); + + diagram_context = diagram->CreateDefaultContext(); + context = + &diagram->GetMutableSubsystemContext(*plant, diagram_context.get()); + context_ad = plant_ad->CreateDefaultContext(); + + // Set up contact geometry pairs + auto left_wall_geoms = + plant->GetCollisionGeometriesForBody(plant->GetBodyByName("left_wall")); + auto right_wall_geoms = plant->GetCollisionGeometriesForBody( + plant->GetBodyByName("right_wall")); + auto pole_geoms = + plant->GetCollisionGeometriesForBody(plant->GetBodyByName("Pole")); + contact_pairs.emplace_back(left_wall_geoms[0], pole_geoms[0]); + contact_pairs.emplace_back(right_wall_geoms[0], pole_geoms[0]); + + // Load controller options + controller_options = drake::yaml::LoadYamlFile( + "examples/resources/cartpole_softwalls/" + "c3_controller_cartpole_options.yaml"); + + // Construct LCSFactorySystem + lcs_factory_system = std::make_unique( + *plant, *context, *plant_ad, *context_ad, contact_pairs, + controller_options.lcs_factory_options); + + lcs_context = lcs_factory_system->CreateDefaultContext(); + lcs_output = lcs_factory_system->AllocateOutput(); + + // Set up dummy state and input + auto state_vec = c3::systems::TimestampedVector( + plant->num_positions() + plant->num_velocities()); + state_vec.SetDataVector(x0); + state_vec.set_timestamp(0.0); + lcs_factory_system->get_input_port_lcs_state().FixValue(lcs_context.get(), + state_vec); + + Eigen::VectorXd u = Eigen::VectorXd::Zero(plant->num_actuators()); + lcs_factory_system->get_input_port_lcs_input().FixValue(lcs_context.get(), + u); + } + + drake::systems::DiagramBuilder builder; + std::unique_ptr> diagram; + std::unique_ptr> diagram_context; + MultibodyPlant* plant{}; + SceneGraph* scene_graph{}; + std::unique_ptr> plant_ad; + drake::systems::Context* context{}; + std::unique_ptr> context_ad; + std::unique_ptr lcs_factory_system; + std::unique_ptr> lcs_context; + std::unique_ptr> lcs_output; + C3ControllerOptions controller_options; + std::vector> contact_pairs; +}; + +TEST_F(LCSFactorySystemTest, InputOutputPortSizes) { + // Check input port sizes and output port existence + EXPECT_EQ(lcs_factory_system->get_input_port_lcs_state().size(), + plant->num_positions() + plant->num_velocities() + + 1); // +1 for timestamp + EXPECT_EQ(lcs_factory_system->get_input_port_lcs_input().size(), + plant->num_actuators()); + EXPECT_NO_THROW(lcs_factory_system->get_output_port_lcs()); + EXPECT_NO_THROW(lcs_factory_system->get_output_port_lcs_contact_jacobian()); +} + +TEST_F(LCSFactorySystemTest, OutputLCSIsValid) { + // Should not throw and should produce an LCS object with correct dimensions + EXPECT_NO_THROW( + { lcs_factory_system->CalcOutput(*lcs_context, lcs_output.get()); }); + const auto& lcs = lcs_output->get_data(0)->get_value(); + EXPECT_EQ(lcs.num_states(), plant->num_positions() + plant->num_velocities()); + EXPECT_EQ(lcs.num_inputs(), plant->num_actuators()); + EXPECT_EQ(lcs.num_lambdas(), LCSFactory::GetNumContactVariables( + controller_options.lcs_factory_options)); + EXPECT_EQ(lcs.dt(), controller_options.lcs_factory_options.dt); + EXPECT_EQ(lcs.N(), controller_options.lcs_factory_options.N); +} + +TEST_F(LCSFactorySystemTest, OutputContactJacobianIsValid) { + // Check that the contact Jacobian output is valid + EXPECT_NO_THROW( + { lcs_factory_system->CalcOutput(*lcs_context, lcs_output.get()); }); + auto [J_lcs, p_lcs] = + lcs_output->get_data(1) + ->get_value< + std::pair>>(); + EXPECT_EQ(p_lcs.size(), contact_pairs.size()); // for two pairs of contacts + EXPECT_EQ(p_lcs.at(0).size(), 3); // 3D coordinate point + EXPECT_EQ(J_lcs.cols(), plant->num_velocities()); + EXPECT_EQ(J_lcs.rows(), contact_pairs.size()); // for frictionless spring +} + +} // namespace test +} // namespace systems +} // namespace c3 diff --git a/tools/scripts/check_format.sh b/tools/scripts/check_format.sh new file mode 100755 index 0000000..fdfd80a --- /dev/null +++ b/tools/scripts/check_format.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Directory to check (can be customized) +DIR="." + +# Find all C++ source/header files +FILES=$(find "$DIR" -type f \( -name "*.cpp" -o -name "*.hpp" -o -name "*.cc" -o -name "*.h" \)) + +NOT_FORMATTED=() + +for file in $FILES; do + # Check formatting using clang-format + if ! diff -q "$file" <(clang-format "$file") >/dev/null; then + NOT_FORMATTED+=("$file") + fi +done + +if [ ${#NOT_FORMATTED[@]} -eq 0 ]; then + echo "🌟 All files are properly formatted. Good job! 🌟" +else + echo "The following files are not properly formatted: 😟" + for f in "${NOT_FORMATTED[@]}"; do + echo "$f" + done + exit 1 +fi \ No newline at end of file