diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml new file mode 100644 index 00000000..f68b3b9d --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -0,0 +1,55 @@ +name: Bug report +description: Report a reproducible defect in the solver, bindings, build, or docs +title: "[bug] " +labels: + - bug +body: + - type: markdown + attributes: + value: | + Thanks for taking the time to report a problem. + + Please include a minimal reproduction and the exact commands you ran. + - type: textarea + id: summary + attributes: + label: Summary + description: What happened, and what did you expect to happen instead? + placeholder: A concise description of the bug and expected behavior. + validations: + required: true + - type: textarea + id: reproduction + attributes: + label: Reproduction + description: Provide the smallest reproduction you can, including commands and inputs. + placeholder: | + 1. Configure with ... + 2. Run ... + 3. Observe ... + validations: + required: true + - type: textarea + id: logs + attributes: + label: Logs and output + description: Paste the relevant error message, stack trace, or failing test output. + render: text + - type: textarea + id: environment + attributes: + label: Environment + description: Share the platform and toolchain details relevant to the bug. + placeholder: | + OS: + Compiler: + CMake: + Python: + Build flags: + validations: + required: true + - type: textarea + id: context + attributes: + label: Additional context + description: Add anything else that might help debug the issue. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 00000000..e504119a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,5 @@ +blank_issues_enabled: true +contact_links: + - name: Security policy + url: https://github.com/astomodynamics/cddp-cpp/blob/master/SECURITY.md + about: Read the security reporting guidance before disclosing a vulnerability. diff --git a/.github/ISSUE_TEMPLATE/feature_request.yml b/.github/ISSUE_TEMPLATE/feature_request.yml new file mode 100644 index 00000000..b59af5ac --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.yml @@ -0,0 +1,36 @@ +name: Feature request +description: Propose an improvement to the solver, examples, bindings, or docs +title: "[feature] " +labels: + - enhancement +body: + - type: markdown + attributes: + value: | + Please describe the problem first, then the change you want. + - type: textarea + id: problem + attributes: + label: Problem or use case + description: What limitation, missing capability, or workflow pain does this address? + placeholder: I am trying to ... + validations: + required: true + - type: textarea + id: proposal + attributes: + label: Proposed change + description: Describe the API, behavior, or workflow you would like to see. + placeholder: Add support for ... + validations: + required: true + - type: textarea + id: alternatives + attributes: + label: Alternatives considered + description: What workarounds or alternative designs have you considered? + - type: textarea + id: context + attributes: + label: Additional context + description: Benchmarks, references, examples, or prior art that help clarify the request. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 00000000..fbfb40b7 --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,16 @@ +## Summary +- Describe the change and why it is needed. + +## Changes +- List the concrete code, API, testing, or documentation updates. + +## Validation +- [ ] `...` + +## Checklist +- [ ] I ran the relevant tests or explained why they were not run. +- [ ] I updated documentation when behavior or public interfaces changed. +- [ ] I kept the change focused and avoided unrelated cleanup. + +## Notes +- Add reviewer context only when it is needed. diff --git a/.github/workflows/PR_TEMPLATE/pull_request_template.md b/.github/workflows/PR_TEMPLATE/pull_request_template.md deleted file mode 100644 index 9ea67c13..00000000 --- a/.github/workflows/PR_TEMPLATE/pull_request_template.md +++ /dev/null @@ -1,14 +0,0 @@ -## Summary - -- - -## Changes - -- - -## Test Plan - -- [ ] - -## Notes - diff --git a/.gitignore b/.gitignore index a5716e3c..243ac7e0 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,7 @@ build/ # Output plots/ -results/frames/ +results/ # Models models/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 3356a3c5..943047c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -157,7 +157,6 @@ set(dynamics_model_srcs src/dynamics_model/spacecraft_nonlinear.cpp src/dynamics_model/dreyfus_rocket.cpp src/dynamics_model/spacecraft_landing2d.cpp - src/dynamics_model/spacecraft_roe.cpp src/dynamics_model/lti_system.cpp src/dynamics_model/spacecraft_twobody.cpp src/dynamics_model/usv_3dof.cpp diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 00000000..a86031d3 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,27 @@ +# Code of Conduct + +## Our standard + +This project expects respectful, technical, and constructive collaboration. + +Examples of expected behavior: + +- focusing feedback on the code, design, and evidence +- being clear, patient, and specific in reviews and discussions +- assuming good intent while still challenging weak technical arguments +- helping other contributors understand project constraints and expectations + +Examples of unacceptable behavior: + +- personal attacks, harassment, or discriminatory language +- hostile or deliberately dismissive review behavior +- repeated bad-faith argumentation or thread derailment +- publishing private or sensitive information without permission + +## Enforcement + +Project maintainers may remove, edit, or reject comments, issues, pull requests, code, and other contributions that violate this code of conduct. + +## Reporting + +If you experience or witness unacceptable behavior, contact the maintainers through the repository owners' preferred contact path. For security-sensitive matters, use the process described in [SECURITY.md](SECURITY.md). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..8099370e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,59 @@ +# Contributing + +Thanks for contributing to `cddp-cpp`. + +## Scope + +This project contains: + +- the core C++ trajectory optimization library +- a curated set of C++ reference examples +- optional Python bindings and portfolio demos + +Keep changes focused. Avoid mixing bug fixes, refactors, formatting churn, and documentation cleanup in one pull request unless they are tightly coupled. + +## Development setup + +### C++ build + +```bash +cmake -S . -B build +cmake --build build -j4 +ctest --test-dir build --output-on-failure +``` + +### C++ examples + +```bash +cmake -S . -B build -DCDDP_CPP_BUILD_EXAMPLES=ON +cmake --build build --target cddp_pendulum cddp_cartpole cddp_unicycle cddp_quadrotor_point cddp_manipulator -j4 +``` + +### Python bindings + +This repository uses `uv` for Python environment management. + +```bash +uv venv .venv --python 3.12 +uv sync +source .venv/bin/activate +pytest -q python/tests +``` + +## Pull requests + +Before opening a pull request: + +1. Make the smallest reasonable change for the problem you are solving. +2. Add or update tests when behavior changes. +3. Run the relevant build and test commands for the area you touched. +4. Update documentation when user-facing behavior, examples, or public APIs change. +5. Write a clear commit message and PR description. + +PRs that include a minimal reproduction, exact validation commands, and a concise explanation of the design tradeoffs are much easier to review. + +## Issues + +- Use bug reports for reproducible defects. +- Use feature requests for API additions, solver improvements, or workflow changes. +- For sensitive security issues, follow [SECURITY.md](SECURITY.md) instead of opening a detailed public issue. diff --git a/README.md b/README.md index 825d4f64..5cb6d3b9 100644 --- a/README.md +++ b/README.md @@ -29,15 +29,57 @@ $$ $$ ## Examples -The default C++ build currently includes a barrier-strategy comparison example: +The maintained example surface is now split: + +* a small C++ reference set in `examples/`, built when `CDDP_CPP_BUILD_EXAMPLES=ON` +* the Python portfolio for plotting, animation, and notebook workflows + +The kept C++ examples are: + +* `cddp_pendulum.cpp` +* `cddp_cartpole.cpp` +* `cddp_unicycle.cpp` +* `cddp_quadrotor_point.cpp` +* `cddp_manipulator.cpp` + +The wider historical C++ example inventory has been removed to keep the example +surface focused. The kept C++ examples are intentionally minimal and do not depend on +visualization libraries. + +### Python Portfolio +The Python bindings now ship with a small animation-focused portfolio built on +top of the same solver models used by the C++ examples: ```bash -./examples/test_barrier_strategies +source .venv/bin/activate +python examples/python_portfolio.py --demo all ``` -Several visualization-focused C++ examples remain in the repository, but they -are not part of the default build. Use the Python bindings for plotting and -notebook workflows. +This generates GIFs under `docs/assets/python_portfolio/` for: + +* pendulum swing-up +* cart-pole swing-up +* unicycle obstacle avoidance +* full-lap MPCC racing line tracking + +See [docs/python_portfolio.md](docs/python_portfolio.md) for the gallery and +regeneration command. + +Pendulum swing-up: +Python pendulum swing-up portfolio demo + +Cart-pole swing-up: +Python cart-pole swing-up portfolio demo + +Unicycle obstacle avoidance: +Python unicycle obstacle avoidance portfolio demo + +MPCC racing line tracking: +Python MPCC racing line portfolio demo + +The MPCC portfolio example is a lightweight kinematic contouring-control demo. +Its vendored track data in `examples/data/` is derived from the +[`alexliniger/MPCC`](https://github.com/alexliniger/MPCC) project. ## Installation ### Dependencies @@ -69,8 +111,7 @@ If you want to use this library for ROS2 MPC node, please refer [CDDP MPC Packag ## References * Y. Tassa, N. Mansard and E. Todorov, "Control-limited differential dynamic programming," 2014 IEEE International Conference on Robotics and Automation (ICRA), 2014, pp. 1168-1175, doi: <10.1109/ICRA.2014.6907001>. * Pavlov, A., Shames, I., and Manzie, C., “Interior Point Differential Dynamic Programming,” IEEE Transactions on Control Systems Technology, Vol. 29, No. 6, 2021, pp. 2720–2727. -* Yuval Tassa's iLQG/DDP trajectory optimization: -* Andrei Pavlov's GitHub repository: +* Liniger, A., Domahidi, A., and Morari, M., “Optimization-based autonomous racing of 1:43 scale RC cars,” Optimal Control Applications and Methods, 2015. doi: <10.1002/oca.2123>. ## Third Party Libraries @@ -93,15 +134,13 @@ This project is licensed under the Apache License 2.0 - see the [LICENSE](LICENS ## Collaboration Contributions are welcome. -If you'd like to contribute: -1. Fork the repository. -2. Create a branch for your change, for example `feature/your-feature-name` or `fix/your-bug-fix`. -3. Make the change and add or update tests as needed. -4. Commit with a descriptive message. -5. Push the branch. -6. Open a pull request against `master`. +Start with: + +- [CONTRIBUTING.md](CONTRIBUTING.md) for setup, validation, and PR expectations +- [CODE_OF_CONDUCT.md](CODE_OF_CONDUCT.md) for community standards +- [SECURITY.md](SECURITY.md) for vulnerability reporting guidance -Use GitHub issues for bug reports, questions, or proposed changes. +Use GitHub issues for bug reports and feature requests, and open pull requests against `master`. ## TODO * improve python binding ergonomics diff --git a/SECURITY.md b/SECURITY.md new file mode 100644 index 00000000..c47348ba --- /dev/null +++ b/SECURITY.md @@ -0,0 +1,26 @@ +# Security Policy + +## Supported versions + +Security fixes are best-effort and are typically made against the current `master` branch. + +## Reporting a vulnerability + +Please do not publish vulnerability details in a public issue. + +Preferred process: + +1. Use GitHub's private vulnerability reporting flow for this repository if it is enabled. +2. If private reporting is not available, open a minimal public issue requesting a secure reporting path, without including exploit details, secrets, or full reproduction steps. + +When reporting an issue, include: + +- affected version or commit +- impact and attack surface +- reproduction steps +- proof-of-concept details only through a private channel +- any suggested mitigation or patch direction + +## Disclosure + +Please allow maintainers reasonable time to investigate and prepare a fix before public disclosure. diff --git a/docs/assets/python_portfolio/cartpole_swing_up.gif b/docs/assets/python_portfolio/cartpole_swing_up.gif new file mode 100644 index 00000000..e79ccac8 Binary files /dev/null and b/docs/assets/python_portfolio/cartpole_swing_up.gif differ diff --git a/docs/assets/python_portfolio/mpcc_racing_line.gif b/docs/assets/python_portfolio/mpcc_racing_line.gif new file mode 100644 index 00000000..cf443cf5 Binary files /dev/null and b/docs/assets/python_portfolio/mpcc_racing_line.gif differ diff --git a/docs/assets/python_portfolio/pendulum_swing_up.gif b/docs/assets/python_portfolio/pendulum_swing_up.gif new file mode 100644 index 00000000..5dc49e2e Binary files /dev/null and b/docs/assets/python_portfolio/pendulum_swing_up.gif differ diff --git a/docs/assets/python_portfolio/unicycle_obstacle_avoidance.gif b/docs/assets/python_portfolio/unicycle_obstacle_avoidance.gif new file mode 100644 index 00000000..053c5483 Binary files /dev/null and b/docs/assets/python_portfolio/unicycle_obstacle_avoidance.gif differ diff --git a/docs/python_portfolio.md b/docs/python_portfolio.md new file mode 100644 index 00000000..63e687bb --- /dev/null +++ b/docs/python_portfolio.md @@ -0,0 +1,48 @@ +# Python Portfolio + +This gallery is the Python-facing showcase for the `pycddp` bindings. Each +demo is solved directly from Python, then rendered as an animation with +Matplotlib. + +## Regenerate the gallery + +```bash +source .venv/bin/activate +python examples/python_portfolio.py --demo all --output-dir docs/assets/python_portfolio +``` + +## Demos + +### Pendulum Swing-Up + +Torque-limited `CLDDP` solve for a damped pendulum. + +Pendulum swing-up animation + +### Cart-Pole Swing-Up + +Bounded-force `CLDDP` solve that swings the pole upright and settles near the +origin. + +Cart-pole swing-up animation + +### Unicycle Obstacle Avoidance + +`IPDDP` solve with a circular obstacle constraint, showing how the Python +bindings can be used for constrained trajectory visualization without building +the legacy C++ plotting stack. + +Unicycle obstacle avoidance animation + +### MPCC Racing Line + +Full-lap receding-horizon contouring control on a compact racing circuit, +solved from Python with a custom nonlinear objective and a lightweight bicycle +model. + +This is a compact kinematic MPCC-style showcase rather than a full reproduction +of the autonomous-racing controller in Liniger et al. The bundled track data is +derived from the [`alexliniger/MPCC`](https://github.com/alexliniger/MPCC) +reference implementation. + +MPCC racing line animation diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f34c5263..6e4a576d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -12,7 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Build only the maintained example used in the default C++ configuration. +# Curated native reference examples that are built with +# `CDDP_CPP_BUILD_EXAMPLES=ON`. These are compile-checked examples of the +# public C++ API, while the Python portfolio remains the visualization path. -add_executable(test_barrier_strategies test_barrier_strategies.cpp) -target_link_libraries(test_barrier_strategies cddp) +function(add_cddp_reference_example target_name source_name) + add_executable(${target_name} ${source_name}) + target_link_libraries(${target_name} PRIVATE cddp) +endfunction() + +add_cddp_reference_example(cddp_pendulum cddp_pendulum.cpp) +add_cddp_reference_example(cddp_cartpole cddp_cartpole.cpp) +add_cddp_reference_example(cddp_unicycle cddp_unicycle.cpp) +add_cddp_reference_example(cddp_quadrotor_point cddp_quadrotor_point.cpp) +add_cddp_reference_example(cddp_manipulator cddp_manipulator.cpp) diff --git a/examples/cddp_acrobot.cpp b/examples/cddp_acrobot.cpp deleted file mode 100644 index b1f00a75..00000000 --- a/examples/cddp_acrobot.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* - Copyright 2025 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "dynamics_model/acrobot.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -int main() { - int state_dim = 4; - int control_dim = 1; - int horizon = 100; - double timestep = 0.05; - double Tf = timestep * horizon; // Total time - - double l1 = 1.0; - double l2 = 1.0; - double m1 = 1.0; - double m2 = 1.0; - double J1 = 1.0; - double J2 = 1.0; - std::string integration_type = "rk4"; - - std::unique_ptr system = std::make_unique( - timestep, l1, l2, m1, m2, J1, J2, integration_type); - - // Cost matrices - Eigen::MatrixXd Q = 10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); - Eigen::MatrixXd R = 1e-2 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = 100.0 * Q; - - // Goal state - Eigen::VectorXd goal_state(state_dim); - goal_state << M_PI/2.0, 0.0, 0.0, 0.0; - - std::vector empty_reference_states; - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << -M_PI/2.0, 0.0, 0.0, 0.0; - - // Create objective - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Control constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -15.0; - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 15.0; - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 200; - options.tolerance = 1e-3; - options.regularization.initial_value = 1e-4; - options.use_ilqr = true; - options.enable_parallel = true; - options.num_threads = 10; - options.debug = false; - options.ipddp.barrier.mu_initial = 1e-1; - - // Create CDDP solver - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Add control constraint - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // Initial trajectory - std::vector X_init(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U_init(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Rollout initial trajectory - X_init[0] = initial_state; - auto acrobot = std::make_unique(timestep, l1, l2, m1, m2, J1, J2, integration_type); - for (int t = 0; t < horizon; ++t) { - X_init[t + 1] = acrobot->getDiscreteDynamics(X_init[t], U_init[t], t * timestep); - } - - cddp_solver.setInitialTrajectory(X_init, U_init); - - // Solve - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Create plot directory - const std::string plotDirectory = "../results/acrobot"; - cddp::example::ensurePlotDir(plotDirectory); - - // Extract solution data for animation - const auto& time_arr = t_sol; - auto theta1_arr = cddp::example::extractComponent(X_sol, 0); - auto theta2_arr = cddp::example::extractComponent(X_sol, 1); - - // --- Animation --- - auto fig = figure(); - auto ax = fig->current_axes(); - fig->size(800, 800); - - // Generate animation frames - for (size_t i = 0; i < X_sol.size(); i += 5) { - cla(ax); - hold(ax, true); - - // Current state - double theta1 = theta1_arr[i]; - double theta2 = theta2_arr[i]; - - // Link 1 endpoint - double x1 = l1 * std::sin(theta1); - double y1 = -l1 * std::cos(theta1); - - // Link 2 endpoint (relative to link 1) - double x2 = x1 + l2 * std::sin(theta1 + theta2); - double y2 = y1 - l2 * std::cos(theta1 + theta2); - - // Plot link 1 - std::vector link1_x = {0.0, x1}; - std::vector link1_y = {0.0, y1}; - auto link1 = plot(link1_x, link1_y); - link1->line_style("b-"); - link1->line_width(5); - - // Plot link 2 - std::vector link2_x = {x1, x2}; - std::vector link2_y = {y1, y2}; - auto link2 = plot(link2_x, link2_y); - link2->line_style("r-"); - link2->line_width(5); - - // Plot joints as circles - std::vector joint0_x = {0.0}; - std::vector joint0_y = {0.0}; - auto j0 = scatter(joint0_x, joint0_y); - j0->marker_size(10); - j0->marker_color("black"); - j0->marker_style("o"); - - std::vector joint1_x = {x1}; - std::vector joint1_y = {y1}; - auto j1 = scatter(joint1_x, joint1_y); - j1->marker_size(8); - j1->marker_color("gray"); - j1->marker_style("o"); - - std::vector joint2_x = {x2}; - std::vector joint2_y = {y2}; - auto j2 = scatter(joint2_x, joint2_y); - j2->marker_size(6); - j2->marker_color("red"); - j2->marker_style("o"); - - // Set axis properties - xlim({-2.5, 2.5}); - ylim({-2.5, 2.5}); - xlabel("x [m]"); - ylabel("y [m]"); - title("Acrobot Animation - Time: " + std::to_string(time_arr[i]) + " s"); - grid(true); - - // Save frame - std::string filename = plotDirectory + "/frame_" + std::to_string(i/5) + ".png"; - fig->save(filename); - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool - std::string command = "convert -delay 30 " + plotDirectory + "/frame_*.png " + plotDirectory + "/acrobot.gif"; - std::system(command.c_str()); - - // Clean up frame files - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as " << plotDirectory << "/acrobot.gif" << std::endl; - - // Print final state - std::cout << "\nFinal state:" << std::endl; - std::cout << "θ₁ = " << X_sol.back()(0) << " rad" << std::endl; - std::cout << "θ₂ = " << X_sol.back()(1) << " rad" << std::endl; - std::cout << "θ̇₁ = " << X_sol.back()(2) << " rad/s" << std::endl; - std::cout << "θ̇₂ = " << X_sol.back()(3) << " rad/s" << std::endl; - - return 0; -} \ No newline at end of file diff --git a/examples/cddp_bicycle.cpp b/examples/cddp_bicycle.cpp deleted file mode 100644 index 5d2c6a59..00000000 --- a/examples/cddp_bicycle.cpp +++ /dev/null @@ -1,232 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -int main() -{ - // Problem parameters - int state_dim = 4; // [x, y, theta, v] - int control_dim = 2; // [acceleration, steering_angle] - int horizon = 100; - double timestep = 0.05; - std::string integration_type = "euler"; - - // Create a bicycle instance - double wheelbase = 1.5; // wheelbase length in meters - std::unique_ptr system = std::make_unique(timestep, wheelbase, integration_type); - - // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 50.0, 0.0, 0.0, 0.0, - 0.0, 50.0, 0.0, 0.0, - 0.0, 0.0, 10.0, 0.0, - 0.0, 0.0, 0.0, 10.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 5.0, 5.0, M_PI / 2.0, 0.0; - - std::vector empty_reference_states; - auto objective = std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI / 4.0, 0.0; - - // Set solver options - cddp::CDDPOptions options; - options.max_iterations = 20; - - // Create CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Define and add control constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -10.0, -M_PI / 5; - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 10.0, M_PI / 5; - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - auto constraint = cddp_solver.getConstraint("ControlConstraint"); - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - - // Extract solution - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void) std::system("mkdir -p frames"); - - // Extract trajectory data - auto x_arr = cddp::example::extractComponent(X_sol, 0); - auto y_arr = cddp::example::extractComponent(X_sol, 1); - auto theta_arr = cddp::example::extractComponent(X_sol, 2); - auto v_arr = cddp::example::extractComponent(X_sol, 3); - - // Extract control inputs - auto acc_arr = cddp::example::extractComponent(U_sol, 0); - auto steering_arr = cddp::example::extractComponent(U_sol, 1); - - // ----------------------------- - // Plot states and controls - // ----------------------------- - auto f1 = figure(); - f1->size(1200, 800); - - // First subplot: Position Trajectory - auto ax1 = subplot(2, 2, 0); - auto plot_handle = plot(ax1, x_arr, y_arr, "-b"); - plot_handle->line_width(3); - title(ax1, "Position Trajectory"); - xlabel(ax1, "x [m]"); - ylabel(ax1, "y [m]"); - - // Second subplot: Heading Angle vs Time - auto ax2 = subplot(2, 2, 1); - auto heading_plot_handle = plot(ax2, t_sol, theta_arr); - heading_plot_handle->line_width(3); - title(ax2, "Heading Angle"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "theta [rad]"); - - // Third subplot: Velocity vs Time - auto ax3 = subplot(2, 2, 2); - auto velocity_plot_handle = plot(ax3, t_sol, v_arr); - velocity_plot_handle->line_width(3); - title(ax3, "Velocity"); - xlabel(ax3, "Time [s]"); - ylabel(ax3, "v [m/s]"); - - // Fourth subplot: Control Inputs - auto ax4 = subplot(2, 2, 3); - auto p1 = plot(ax4, acc_arr, "--b"); - p1->line_width(3); - p1->display_name("Acceleration"); - - hold(ax4, true); - auto p2 = plot(ax4, steering_arr, "--r"); - p2->line_width(3); - p2->display_name("Steering"); - - title(ax4, "Control Inputs"); - xlabel(ax4, "Step"); - ylabel(ax4, "Control"); - matplot::legend(ax4); - - f1->draw(); - f1->save(plotDirectory + "/bicycle_cddp_results.png"); - - // ----------------------------- - // Animation: Bicycle Trajectory - // ----------------------------- - auto f2 = figure(); - f2->size(800, 600); - auto ax_anim = f2->current_axes(); - if (!ax_anim) - { - ax_anim = axes(); - } - - double car_length = 0.35; - double car_width = 0.15; - - for (size_t i = 0; i < X_sol.size(); ++i) - { - if (i % 10 == 0) - { - ax_anim->clear(); - hold(ax_anim, true); - - double x = x_arr[i]; - double y = y_arr[i]; - double theta = theta_arr[i]; - - // Compute bicycle rectangle corners - std::vector car_x(5), car_y(5); - car_x[0] = x + car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[0] = y + car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[1] = x + car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[1] = y + car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[2] = x - car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[2] = y - car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[3] = x - car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[3] = y - car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - auto car_line = plot(ax_anim, car_x, car_y); - car_line->color("black"); - car_line->line_style("solid"); - car_line->line_width(2); - car_line->display_name("Car"); - - // Plot the front wheel (steerable) - double wheel_length = car_width * 0.8; - double steering_angle = U_sol[std::min(i, U_sol.size() - 1)](1); - std::vector front_wheel_x = { - x + car_length / 2 * cos(theta), - x + car_length / 2 * cos(theta) + wheel_length * cos(theta + steering_angle)}; - std::vector front_wheel_y = { - y + car_length / 2 * sin(theta), - y + car_length / 2 * sin(theta) + wheel_length * sin(theta + steering_angle)}; - auto front_wheel_line = plot(ax_anim, front_wheel_x, front_wheel_y); - front_wheel_line->color("red"); - front_wheel_line->line_style("solid"); - front_wheel_line->line_width(2); - front_wheel_line->display_name(""); - - // Plot trajectory up to current frame - std::vector traj_x(x_arr.begin(), x_arr.begin() + i + 1); - std::vector traj_y(y_arr.begin(), y_arr.begin() + i + 1); - auto traj_line = plot(ax_anim, traj_x, traj_y); - traj_line->color("blue"); - traj_line->line_style("solid"); - traj_line->line_width(1.5); - traj_line->display_name("Trajectory"); - - title(ax_anim, "Bicycle Trajectory"); - xlabel(ax_anim, "x [m]"); - ylabel(ax_anim, "y [m]"); - xlim(ax_anim, {-1, 6}); - ylim(ax_anim, {-1, 6}); - // legend(ax_anim); - - std::string filename = plotDirectory + "/bicycle_frame_" + std::to_string(i) + ".png"; - f2->draw(); - f2->save(filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/bicycle_frame_*.png " + plotDirectory + "/bicycle.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/bicycle_frame_*.png"; - std::system(cleanup_command.c_str()); - - return 0; -} diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp deleted file mode 100644 index 05b7151a..00000000 --- a/examples/cddp_car.cpp +++ /dev/null @@ -1,329 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -namespace cddp -{ - - class CarParkingObjective : public NonlinearObjective - { - public: - CarParkingObjective(const Eigen::VectorXd &goal_state, double timestep) - : NonlinearObjective(timestep), reference_state_(goal_state) - { - // Control cost coefficients: cu = 1e-2*[1 .01] - cu_ = Eigen::Vector2d(1e-2, 1e-4); - - // Final cost coefficients: cf = [.1 .1 1 .3] - cf_ = Eigen::Vector4d(0.1, 0.1, 1.0, 0.3); - - // Smoothness scales for final cost: pf = [.01 .01 .01 1] - pf_ = Eigen::Vector4d(0.01, 0.01, 0.01, 1.0); - - // Running cost coefficients: cx = 1e-3*[1 1] - cx_ = Eigen::Vector2d(1e-3, 1e-3); - - // Smoothness scales for running cost: px = [.1 .1] - px_ = Eigen::Vector2d(0.1, 0.1); - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index) const override - { - // Control cost: lu = cu*u.^2 - double lu = cu_.dot(control.array().square().matrix()); - - // Running cost on distance from origin: lx = cx*sabs(x(1:2,:),px) - Eigen::VectorXd xy_state = state.head(2); - double lx = cx_.dot(sabs(xy_state, px_)); - - return lu + lx; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - // Final state cost: llf = cf*sabs(x(:,final),pf); - return cf_.dot(sabs(final_state, pf_)) + running_cost(final_state, Eigen::VectorXd::Zero(2), 0); - } - - private: - // Helper function for smooth absolute value (pseudo-Huber) - Eigen::VectorXd sabs(const Eigen::VectorXd &x, const Eigen::VectorXd &p) const - { - return ((x.array().square() / p.array().square() + 1.0).sqrt() * p.array() - p.array()).matrix(); - } - - Eigen::VectorXd reference_state_; - Eigen::Vector2d cu_; // Control cost coefficients - Eigen::Vector4d cf_; // Final cost coefficients - Eigen::Vector4d pf_; // Smoothness scales for final cost - Eigen::Vector2d cx_; // Running cost coefficients - Eigen::Vector2d px_; // Smoothness scales for running cost - }; -} // namespace cddp - -void plotCarBox(const Eigen::VectorXd &state, const Eigen::VectorXd &control, - double length, double width, const std::string &color, - axes_handle ax) -{ - double x = state(0); - double y = state(1); - double theta = state(2); - double steering = control(1); - - // Compute the car's four corners (and close the polygon) - std::vector car_x(5), car_y(5); - - // Front right - car_x[0] = x + length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[0] = y + length / 2 * sin(theta) + width / 2 * cos(theta); - - // Front left - car_x[1] = x + length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[1] = y + length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear left - car_x[2] = x - length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[2] = y - length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear right - car_x[3] = x - length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[3] = y - length / 2 * sin(theta) + width / 2 * cos(theta); - - // Close polygon - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - // Plot car body as a polygon line. - plot(ax, car_x, car_y, color + "-"); - - // Plot base point (center of rear axle) as a red circle. - plot(ax, std::vector{x}, std::vector{y}, "ro"); - - // Compute steering direction - double front_x = x + length / 2 * cos(theta); - double front_y = y + length / 2 * sin(theta); - double steering_length = width / 2; - double steering_angle = theta + steering; - double steering_end_x = front_x + steering_length * cos(steering_angle); - double steering_end_y = front_y + steering_length * sin(steering_angle); - - std::vector steer_x = {front_x, steering_end_x}; - std::vector steer_y = {front_y, steering_end_y}; - plot(ax, steer_x, steer_y, "g-"); -} - -int main() -{ - // Problem parameters - int state_dim = 4; // [x y theta v] - int control_dim = 2; // [wheel_angle acceleration] - int horizon = 500; - double timestep = 0.03; - std::string integration_type = "euler"; - - // Random number generator setup - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution d(0.0, 0.1); - - // Create car instance - double wheelbase = 2.0; - - // Initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << 1.0, 1.0, 1.5 * M_PI, 0.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, 0.0, 0.0; // NOT USED IN THIS EXAMPLE - - // Control constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -0.5, -2.0; // [steering_angle, acceleration] - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 0.5, 2.0; - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 500; - options.verbose = true; - options.tolerance = 1e-7; - options.acceptable_tolerance = 1e-4; - options.regularization.initial_value = 1e-7; - options.debug = false; - options.enable_parallel = true; - options.num_threads = 10; - - // Create CDDP solver with new API - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, wheelbase, integration_type), - std::make_unique(goal_state, timestep), - options - ); - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique( - control_lower_bound, control_upper_bound)); - - // Initialize with random controls - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Random initialization - for (auto &u : U) - { - // u(0) = d(gen); // Random steering - // u(1) = d(gen); // Random acceleration - u(0) = 0.01; - u(1) = 0.01; - } - - X[0] = initial_state; - - double J = 0.0; - - // Simulate initial trajectory - for (size_t t = 0; t < horizon; t++) - { - J += cddp_solver.getObjective().running_cost(X[t], U[t], t); - X[t + 1] = cddp_solver.getSystem().getDiscreteDynamics(X[t], U[t], t * timestep); - } - J += cddp_solver.getObjective().terminal_cost(X.back()); - std::cout << "Initial cost: " << J << std::endl; - std::cout << "Initial state: " << X[0].transpose() << std::endl; - std::cout << "Final state: " << X.back().transpose() << std::endl; - - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - // ======================================== - // CDDP Solution - // ======================================== - // Converged: Yes - // Iterations: 157 - // Solve Time: 5.507e+05 micro sec - // Final Cost: 1.90517 - // ======================================== - // cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::LogDDP); - // ======================================== - // CDDP Solution - // ======================================== - // Converged: Yes - // Iterations: 153 - // Solve Time: 5.441e+05 micro sec - // Final Cost: 1.90517 - // ======================================== - // ======================================== - // CDDP Solution - // ======================================== - // Converged: No - // Iterations: 53 - // Solve Time: 1.1538e+06 micro sec - // Final Cost: 1.910013e+00 - // ======================================== - - // Extract solution trajectories - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Prepare trajectory data - auto x_hist = cddp::example::extractComponent(X_sol, 0); - auto y_hist = cddp::example::extractComponent(X_sol, 1); - - // Car dimensions. - double car_length = 2.1; - double car_width = 0.9; - - // Create a figure and get current axes. - auto fig = figure(true); - auto ax = fig->current_axes(); - - Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(2); - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void) std::system("mkdir -p frames"); - - // Animation loop: update plot for each time step and save frame. - for (size_t i = 0; i < X_sol.size(); ++i) - { - // Skip every 10th frame for smoother animation. - if (i % 10 == 0) - { - // Clear previous content. - cla(ax); - hold(ax, true); - - // Plot the full trajectory. - plot(ax, x_hist, y_hist, "b-"); - - // Plot goal configuration. - plotCarBox(goal_state, empty_control, car_length, car_width, "r", ax); - - // Plot current car state. - if (i < U_sol.size()) - plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k", ax); - else - plotCarBox(X_sol[i], empty_control, car_length, car_width, "k", ax); - - // Set grid and axis limits. - grid(ax, true); - xlim(ax, {-4, 4}); - ylim(ax, {-4, 4}); - - // Update drawing. - fig->draw(); - - // Save the frame to a PNG file. - std::string frame_filename = plotDirectory + "/frame_" + std::to_string(i) + ".png"; - fig->save(frame_filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 15 " + plotDirectory + "/frame_*.png " + plotDirectory + "/car_parking.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as car_parking.gif" << std::endl; - - return 0; -} - -// convert -delay 3 ../results/frames/frame_*.png ../results/animations/car_parking.gif diff --git a/examples/cddp_car_ipddp.cpp b/examples/cddp_car_ipddp.cpp deleted file mode 100644 index 044cbef5..00000000 --- a/examples/cddp_car_ipddp.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -namespace cddp -{ - - class CarParkingObjective : public NonlinearObjective - { - public: - CarParkingObjective(const Eigen::VectorXd &goal_state, double timestep) - : NonlinearObjective(timestep), reference_state_(goal_state) - { - // Control cost coefficients: cu = 1e-2*[1 .01] - cu_ = Eigen::Vector2d(1e-2, 1e-4); - - // Final cost coefficients: cf = [.1 .1 1 .3] - cf_ = Eigen::Vector4d(0.1, 0.1, 1.0, 0.3); - - // Smoothness scales for final cost: pf = [.01 .01 .01 1] - pf_ = Eigen::Vector4d(0.01, 0.01, 0.01, 1.0); - - // Running cost coefficients: cx = 1e-3*[1 1] - cx_ = Eigen::Vector2d(1e-3, 1e-3); - - // Smoothness scales for running cost: px = [.1 .1] - px_ = Eigen::Vector2d(0.1, 0.1); - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index) const override - { - // Control cost: lu = cu*u.^2 - double lu = cu_.dot(control.array().square().matrix()); - - // Running cost on distance from origin: lx = cx*sabs(x(1:2,:),px) - Eigen::VectorXd xy_state = state.head(2); - double lx = cx_.dot(sabs(xy_state, px_)); - - return lu + lx; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - // Final state cost: llf = cf*sabs(x(:,final),pf); - return cf_.dot(sabs(final_state, pf_)) + running_cost(final_state, Eigen::VectorXd::Zero(2), 0); - } - - private: - // Helper function for smooth absolute value (pseudo-Huber) - Eigen::VectorXd sabs(const Eigen::VectorXd &x, const Eigen::VectorXd &p) const - { - return ((x.array().square() / p.array().square() + 1.0).sqrt() * p.array() - p.array()).matrix(); - } - - Eigen::VectorXd reference_state_; - Eigen::Vector2d cu_; // Control cost coefficients - Eigen::Vector4d cf_; // Final cost coefficients - Eigen::Vector4d pf_; // Smoothness scales for final cost - Eigen::Vector2d cx_; // Running cost coefficients - Eigen::Vector2d px_; // Smoothness scales for running cost - }; -} // namespace cddp - -void plotCarBox(const Eigen::VectorXd &state, const Eigen::VectorXd &control, - double length, double width, const std::string &color, - axes_handle ax) -{ - double x = state(0); - double y = state(1); - double theta = state(2); - double steering = control(1); - - // Compute the car's four corners (and close the polygon) - std::vector car_x(5), car_y(5); - - // Front right - car_x[0] = x + length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[0] = y + length / 2 * sin(theta) + width / 2 * cos(theta); - - // Front left - car_x[1] = x + length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[1] = y + length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear left - car_x[2] = x - length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[2] = y - length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear right - car_x[3] = x - length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[3] = y - length / 2 * sin(theta) + width / 2 * cos(theta); - - // Close polygon - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - // Plot car body as a polygon line. - plot(ax, car_x, car_y, color + "-"); - - // Plot base point (center of rear axle) as a red circle. - plot(ax, std::vector{x}, std::vector{y}, "ro"); - - // Compute steering direction - double front_x = x + length / 2 * cos(theta); - double front_y = y + length / 2 * sin(theta); - double steering_length = width / 2; - double steering_angle = theta + steering; - double steering_end_x = front_x + steering_length * cos(steering_angle); - double steering_end_y = front_y + steering_length * sin(steering_angle); - - std::vector steer_x = {front_x, steering_end_x}; - std::vector steer_y = {front_y, steering_end_y}; - plot(ax, steer_x, steer_y, "g-"); -} - - -int main() { - // Problem parameters - const int state_dim = 4; // [x, y, theta, v] - const int control_dim = 2; // [wheel_angle, acceleration] - const int horizon = 500; - const double timestep = 0.03; - const std::string integration_type = "euler"; - - // Create a Car instance with given parameters - double wheelbase = 2.0; - std::unique_ptr system = - std::make_unique(timestep, wheelbase, integration_type); - - // Define initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << 1.0, 1.0, 1.5 * M_PI, 0.0; - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, 0.0, 0.0; // Desired parking state - - // Create the nonlinear objective for car parking - auto objective = std::make_unique(goal_state, timestep); - - // Set solver options - cddp::CDDPOptions options; - options.max_iterations = 600; - options.verbose = true; - options.tolerance = 1e-7; - options.acceptable_tolerance = 1e-4; - options.regularization.initial_value = 1e-7; - options.debug = false; - options.use_ilqr = true; - options.enable_parallel = false; - options.num_threads = 1; - options.msipddp.barrier.mu_initial = 1e-0; - options.msipddp.dual_var_init_scale = 1e-1; - options.msipddp.slack_var_init_scale = 1e-2; - options.msipddp.segment_length = horizon / 100; - options.msipddp.rollout_type = "nonlinear"; - - // Create CDDP solver for the car model with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Define control constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -0.5, -2.0; // [steering_angle, acceleration] - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 0.5, 2.0; - cddp_solver.addPathConstraint("ControlConstraint", std::make_unique(-control_upper_bound, control_upper_bound)); - - // Initialize the trajectory with zero controls - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - for (int i = 0; i < horizon; ++i) { - U[i](0) = 0.01; - U[i](1) = 0.01; - X[i + 1] = cddp_solver.getSystem().getDiscreteDynamics(X[i], U[i], i * timestep); - } - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem using MSIPDDP - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - - // Extract solution trajectories - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Prepare trajectory data for plotting - auto x_hist = cddp::example::extractComponent(X_sol, 0); - auto y_hist = cddp::example::extractComponent(X_sol, 1); - // Car dimensions. - double car_length = 2.1; - double car_width = 0.9; - - // Create a figure and get current axes. - auto fig = figure(true); - auto ax = fig->current_axes(); - - Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(2); - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void) std::system("mkdir -p frames"); - - // Animation loop: update plot for each time step and save frame. - for (size_t i = 0; i < X_sol.size(); ++i) - { - // Skip every 10th frame for smoother animation. - if (i % 10 == 0) - { - // Clear previous content. - cla(ax); - hold(ax, true); - - // Plot the full trajectory. - plot(ax, x_hist, y_hist, "b-"); - - // Plot goal configuration. - plotCarBox(goal_state, empty_control, car_length, car_width, "r", ax); - - // Plot current car state. - if (i < U_sol.size()) - plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k", ax); - else - plotCarBox(X_sol[i], empty_control, car_length, car_width, "k", ax); - - // Set grid and axis limits. - grid(ax, true); - xlim(ax, {-4, 4}); - ylim(ax, {-4, 4}); - - // Update drawing. - fig->draw(); - - // Save the frame to a PNG file. - std::string frame_filename = plotDirectory + "/frame_" + std::to_string(i) + ".png"; - fig->save(frame_filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 15 " + plotDirectory + "/frame_*.png " + plotDirectory + "/car_parking_ipddp.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as car_parking_ipddp.gif" << std::endl; - - return 0; -} diff --git a/examples/cddp_cartpole.cpp b/examples/cddp_cartpole.cpp index b459fccb..aca6b3f4 100644 --- a/examples/cddp_cartpole.cpp +++ b/examples/cddp_cartpole.cpp @@ -13,231 +13,68 @@ See the License for the specific language governing permissions and limitations under the License. */ + #include #include -#include -#include -#include -#include + #include "cddp.hpp" #include "cddp_example_utils.hpp" -#include "matplot/matplot.h" -#include - -using namespace matplot; -namespace fs = std::filesystem; int main() { - int state_dim = 4; - int control_dim = 1; - int horizon = 100; - double timestep = 0.05; + constexpr double kPi = 3.14159265358979323846; + constexpr int state_dim = 4; + constexpr int control_dim = 1; + constexpr int horizon = 100; + constexpr double timestep = 0.05; - // Create a CartPole instance with custom parameters. - double cart_mass = 1.0; - double pole_mass = 0.2; - double pole_length = 0.5; - double gravity = 9.81; - double damping = 0.0; // TODO: Implement damping term. - std::string integration_type = "rk4"; + Eigen::VectorXd initial_state(state_dim); + initial_state << 0.0, 0.0, 0.0, 0.0; - std::unique_ptr system = std::make_unique( - timestep, integration_type, cart_mass, pole_mass, pole_length, gravity, damping); + Eigen::VectorXd goal_state(state_dim); + goal_state << 0.0, kPi, 0.0, 0.0; - // Cost matrices. Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf(0,0) = 100.0; // Final cart position cost. - Qf(1,1) = 100.0; // Final cart velocity cost. - Qf(2,2) = 100.0; // Final pole angle cost. - Qf(3,3) = 100.0; // Final pole angular velocity cost. - - // Goal state: cart at origin, pole upright, zero velocities. - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state << 0.0, M_PI, 0.0, 0.0; - - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state (cart at rest, pole hanging down). - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, 0.0, 0.0; + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); - // Solver options. cddp::CDDPOptions options; - options.max_iterations = 500; - options.tolerance = 1e-7; - options.acceptable_tolerance = 1e-6; + options.max_iterations = 80; + options.tolerance = 1e-6; + options.acceptable_tolerance = 1e-5; options.regularization.initial_value = 1e-5; - options.use_ilqr = true; - options.enable_parallel = true; - options.num_threads = 12; - options.debug = false; - options.ipddp.barrier.mu_initial = 1e-1; - options.msipddp.segment_length = horizon; - options.msipddp.rollout_type = "nonlinear"; - options.ipddp.barrier.mu_update_factor = 0.2; - options.ipddp.barrier.mu_update_power = 1.2; - - // Create CDDP solver with new API. - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Control constraints. - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -5.0; // Maximum negative force. - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 5.0; // Maximum positive force. - - // FIXME: For MSIPDDP - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // FIXME: For CLDDP - // cddp_solver.addPathConstraint("ControlConstraint", - // std::make_unique( control_lower_bound, control_upper_bound)); - - // Initial trajectory. - auto [X, U] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); - cddp_solver.setInitialTrajectory(X, U); - - // Solve. - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Create plot directory. - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void) std::system("mkdir -p frames"); - - // Extract solution data. - auto x_arr = cddp::example::extractComponent(X_sol, 0); - auto theta_arr = cddp::example::extractComponent(X_sol, 1); - auto x_dot_arr = cddp::example::extractComponent(X_sol, 2); - auto theta_dot_arr = cddp::example::extractComponent(X_sol, 3); - auto force_arr = cddp::example::extractComponent(U_sol, 0); - const auto& time_arr = t_sol; - std::vector time_arr2(t_sol.begin(), t_sol.begin() + U_sol.size()); - - // --- Plot static results (2x2 plots for state trajectories) --- - auto fig1 = figure(); - fig1->size(1200, 800); - auto ax1 = subplot(2, 2, 1); - title(ax1, "Cart Position"); - plot(ax1, time_arr, x_arr)->line_style("b-"); - xlabel(ax1, "Time [s]"); - ylabel(ax1, "Position [m]"); - grid(ax1, true); - - auto ax2 = subplot(2, 2, 2); - title(ax2, "Cart Velocity"); - plot(ax2, time_arr, x_dot_arr)->line_style("b-"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "Velocity [m/s]"); - grid(ax2, true); - - auto ax3 = subplot(2, 2, 3); - title(ax3, "Pole Angle"); - plot(ax3, time_arr, theta_arr)->line_style("b-"); - xlabel(ax3, "Time [s]"); - ylabel(ax3, "Angle [rad]"); - grid(ax3, true); - - auto ax4 = subplot(2, 2, 4); - title(ax4, "Pole Angular Velocity"); - plot(ax4, time_arr, theta_dot_arr)->line_style("b-"); - xlabel(ax4, "Time [s]"); - ylabel(ax4, "Angular Velocity [rad/s]"); - grid(ax4, true); - - fig1->save(plotDirectory + "/cartpole_results.png"); - - // --- Plot control inputs --- - auto fig2 = figure(); - fig2->size(800, 600); - title("Control Inputs"); - plot(time_arr2, force_arr)->line_style("b-"); - xlabel("Time [s]"); - ylabel("Force [N]"); - grid(true); - fig2->save(plotDirectory + "/cartpole_control_inputs.png"); - - // --- Animation --- - auto fig3 = figure(); - auto ax_fig3 = fig3->current_axes(); - fig3->size(800, 600); - title("CartPole Animation"); - xlabel("x"); - ylabel("y"); - - double cart_width = 0.3; - double cart_height = 0.2; - double pole_width = 0.05; - - // Loop over the solution states to generate animation frames. - for (size_t i = 0; i < X_sol.size(); ++i) { - if (i % 5 == 0) { - // Clear previous content. - cla(ax_fig3); - hold(ax_fig3, true); - - // Current state. - double x = x_arr[i]; - double theta = theta_arr[i]; - - // Plot the cart as a rectangle centered at (x, 0). - std::vector cart_x = { x - cart_width/2, x + cart_width/2, - x + cart_width/2, x - cart_width/2, - x - cart_width/2 }; - std::vector cart_y = { -cart_height/2, -cart_height/2, - cart_height/2, cart_height/2, - -cart_height/2 }; - plot(cart_x, cart_y)->line_style("k-"); - - // Plot the pole as a line from the top center of the cart. - double pole_end_x = x + pole_length * std::sin(theta); - double pole_end_y = cart_height/2 - pole_length * std::cos(theta); - std::vector pole_x = { x, pole_end_x }; - std::vector pole_y = { cart_height/2, pole_end_y }; - plot(pole_x, pole_y)->line_style("b-"); - - // Plot the pole bob as a circle. - std::vector circle_x, circle_y; - int num_points = 20; - for (int j = 0; j <= num_points; ++j) { - double t = 2 * M_PI * j / num_points; - circle_x.push_back(pole_end_x + pole_width * std::cos(t)); - circle_y.push_back(pole_end_y + pole_width * std::sin(t)); - } - plot(circle_x, circle_y)->line_style("b-"); - - // Set fixed axis limits for stable animation. - xlim({-2.0, 2.0}); - ylim({-1.5, 1.5}); - - // Save the frame. - std::string filename = plotDirectory + "/frame_" + std::to_string(i) + ".png"; - fig3->save(filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } + cddp::CDDP solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique( + timestep, "rk4", 1.0, 0.2, 0.5, 9.81, 0.0), + std::make_unique( + Q, R, Qf, goal_state, std::vector{}, timestep), + options); + + Eigen::VectorXd lower(control_dim); + lower << -5.0; + Eigen::VectorXd upper(control_dim); + upper << 5.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(lower, upper)); + + auto [X, U] = + cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); + solver.setInitialTrajectory(X, U); + + const cddp::CDDPSolution solution = solver.solve(cddp::SolverType::IPDDP); + if (solution.state_trajectory.empty()) { + std::cerr << "Cart-pole example failed: empty trajectory" << std::endl; + return 1; } - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 30 " + plotDirectory + "/frame_*.png " + plotDirectory + "/cartpole.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as cartpole.gif" << std::endl; - - + const Eigen::VectorXd final_error = solution.state_trajectory.back() - goal_state; + std::cout << "Cart-pole example completed with status: " + << solution.status_message << '\n' + << "Final objective: " << solution.final_objective << '\n' + << "Final state error norm: " << final_error.norm() << std::endl; return 0; } diff --git a/examples/cddp_forklift_ipddp.cpp b/examples/cddp_forklift_ipddp.cpp deleted file mode 100644 index 17040836..00000000 --- a/examples/cddp_forklift_ipddp.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/* - Copyright 2025 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "dynamics_model/forklift.hpp" -#include "matplot/matplot.h" -#include - -using namespace matplot; -namespace fs = std::filesystem; - -namespace cddp -{ - - class ForkliftParkingObjective : public NonlinearObjective - { - public: - ForkliftParkingObjective(const Eigen::VectorXd &goal_state, double timestep, int horizon) - : NonlinearObjective(timestep), reference_state_(goal_state), horizon_(horizon) - { - // Control cost coefficients - // R matrix: cu = [acceleration, steering_rate] - cu_ = Eigen::Vector2d(1.8, 11.0); - // Terminal cost coefficients: Qf = [x, y, theta, v, delta] - cf_ = Eigen::VectorXd(5); - cf_ << 1000.0, 1000.0, 5000.0, 16.0, 100.0; // From README Table 4 - - // Stage cost coefficients: Q = [x, y, theta, v, delta] - cx_full_ = Eigen::VectorXd(5); - cx_full_ << 1.0, 1.0, 10.0, 4.0, 6.0; // From README Table 4 - - // Time penalty weight - time_weight_ = 0.05; // From README - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index) const override - { - // Control cost: 0.5 * u^T * R * u - double lu = 0.5 * cu_.dot(control.array().square().matrix()); - - // State cost: 0.5 * (x - x_tar)^T * Q * (x - x_tar) - Eigen::VectorXd state_error = state - reference_state_; - double lx = 0.5 * cx_full_.dot(state_error.array().square().matrix()); - - return lu + lx; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - // Terminal cost: 0.5 * (x - x_tar)^T * Qf * (x - x_tar) - Eigen::VectorXd state_error = final_state - reference_state_; - double terminal_cost = 0.5 * cf_.dot(state_error.array().square().matrix()); - - // Add time penalty: w_T * T (simplified as constant per timestep) - double time_cost = time_weight_ * horizon_ * timestep_; - - return terminal_cost + time_cost; - } - - private: - Eigen::VectorXd reference_state_; - Eigen::Vector2d cu_; // Control cost coefficients (R matrix) - Eigen::VectorXd cf_; // Terminal cost coefficients (Qf matrix) - Eigen::VectorXd cx_full_; // Stage cost coefficients (Q matrix) - double time_weight_; // Time penalty weight - int horizon_; // Total horizon length - }; -} // namespace cddp - -void plotForkliftBox(const Eigen::VectorXd &state, const Eigen::VectorXd &control, - double length, double width, bool rear_steer, - const std::string &color, axes_handle ax) -{ - double x = state(0); - double y = state(1); - double theta = state(2); - double steering = state(4); // Steering angle is now a state - - // Compute the forklift's four corners - std::vector forklift_x(5), forklift_y(5); - - // Front right - forklift_x[0] = x + length / 2 * cos(theta) - width / 2 * sin(theta); - forklift_y[0] = y + length / 2 * sin(theta) + width / 2 * cos(theta); - - // Front left - forklift_x[1] = x + length / 2 * cos(theta) + width / 2 * sin(theta); - forklift_y[1] = y + length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear left - forklift_x[2] = x - length / 2 * cos(theta) + width / 2 * sin(theta); - forklift_y[2] = y - length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear right - forklift_x[3] = x - length / 2 * cos(theta) - width / 2 * sin(theta); - forklift_y[3] = y - length / 2 * sin(theta) + width / 2 * cos(theta); - - // Close polygon - forklift_x[4] = forklift_x[0]; - forklift_y[4] = forklift_y[0]; - - // Plot forklift body as a polygon line - plot(ax, forklift_x, forklift_y, color + "-"); - - // Plot base point (center of rear axle) as a red circle - plot(ax, std::vector{x}, std::vector{y}, "ro"); - - // Plot fork position (front of forklift) - double fork_length = width * 0.8; - double fork_x_start = x + length / 2 * cos(theta); - double fork_y_start = y + length / 2 * sin(theta); - double fork_x_end = fork_x_start + fork_length * cos(theta); - double fork_y_end = fork_y_start + fork_length * sin(theta); - - plot(ax, std::vector{fork_x_start, fork_x_end}, - std::vector{fork_y_start, fork_y_end}, "m-"); - - // Plot steering direction - if (rear_steer) { - // For rear-steer, show steering at rear axle - double rear_x = x - length / 2 * cos(theta); - double rear_y = y - length / 2 * sin(theta); - double steering_length = width / 2; - double steering_angle = theta - steering; // Rear steer is opposite - double steering_end_x = rear_x + steering_length * cos(steering_angle); - double steering_end_y = rear_y + steering_length * sin(steering_angle); - - plot(ax, std::vector{rear_x, steering_end_x}, - std::vector{rear_y, steering_end_y}, "g-"); - } else { - // For front-steer, show steering at front axle - double front_x = x + length / 2 * cos(theta); - double front_y = y + length / 2 * sin(theta); - double steering_length = width / 2; - double steering_angle = theta + steering; - double steering_end_x = front_x + steering_length * cos(steering_angle); - double steering_end_y = front_y + steering_length * sin(steering_angle); - - plot(ax, std::vector{front_x, steering_end_x}, - std::vector{front_y, steering_end_y}, "g-"); - } -} - - -int main() { - // Problem parameters - const int state_dim = 5; // [x, y, theta, v, delta] - const int control_dim = 2; // [acceleration, steering_rate] - const int horizon = 600; - const double timestep = 0.03; - const std::string integration_type = "rk4"; - - // Create a Forklift instance - double wheelbase = 1.6; - bool rear_steer = true; - double max_steering_angle = 0.9; - - std::unique_ptr system = - std::make_unique(timestep, wheelbase, integration_type, - rear_steer, max_steering_angle); - - // Define initial and goal states for forklift back-in parking - Eigen::VectorXd initial_state(state_dim); - initial_state << 2.0, 0.5, M_PI, 0.0, 0.0; - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, M_PI/2, 0.0, 0.0; - - // Create the nonlinear objective for forklift parking - auto objective = std::make_unique(goal_state, timestep, horizon); - - // Set solver options for better convergence - cddp::CDDPOptions options; - options.max_iterations = 2000; - options.verbose = true; - options.tolerance = 1e-5; // Less strict tolerance - options.acceptable_tolerance = 1e-5; - options.regularization.initial_value = 1e-6; - options.debug = false; - options.use_ilqr = true; - options.enable_parallel = true; - options.num_threads = 10; - - options.msipddp.barrier.mu_initial = 1e-1; // Lower barrier parameter - options.msipddp.dual_var_init_scale = 1e-1; - options.msipddp.slack_var_init_scale = 1e-2; - options.msipddp.segment_length = horizon; // Longer segments - options.msipddp.rollout_type = "nonlinear"; - - // Create CDDP solver for the forklift model - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Define control constraints from README Table 1 - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -1.5, -0.4; // [acceleration, steering_rate] - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.5, 0.4; - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique( - control_lower_bound, control_upper_bound)); - - // State constraints - Eigen::VectorXd state_upper_bound(2); - state_upper_bound << 2.0, max_steering_angle; - cddp_solver.addPathConstraint("StateConstraint", - std::make_unique(-state_upper_bound, state_upper_bound)); - - // Initialize trajectory for back-in parking maneuver - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - - for (int i = 0; i < horizon; ++i) { - // Randomize controls - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_real_distribution<> dis(-0.01, 0.01); - U[i] << dis(gen), dis(gen); - X[i + 1] = cddp_solver.getSystem().getDiscreteDynamics(X[i], U[i], i * timestep); - } - - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem using MSIPDDP - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - - // Extract solution trajectories - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - // Prepare trajectory data for plotting - auto x_hist = cddp::example::extractComponent(X_sol, 0); - auto y_hist = cddp::example::extractComponent(X_sol, 1); - - // Forklift dimensions - double forklift_length = 2.5; // Overall length - double forklift_width = 1.1; // Chassis width - - // Create a figure and get current axes - auto fig = figure(true); - auto ax = fig->current_axes(); - - Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(2); - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Animation loop: update plot for each time step and save frame - for (size_t i = 0; i < X_sol.size(); ++i) - { - // Skip every 10th frame for smoother animation - if (i % 10 == 0) - { - // Clear previous content - cla(ax); - hold(ax, true); - - // Plot the full trajectory - plot(ax, x_hist, y_hist, "b--"); - - // Plot goal configuration - plotForkliftBox(goal_state, empty_control, forklift_length, - forklift_width, rear_steer, "r", ax); - - // Plot current forklift state - if (i < U_sol.size()) - plotForkliftBox(X_sol[i], U_sol[i], forklift_length, - forklift_width, rear_steer, "k", ax); - else - plotForkliftBox(X_sol[i], empty_control, forklift_length, - forklift_width, rear_steer, "k", ax); - - // Add labels and title - xlabel(ax, "X [m]"); - ylabel(ax, "Y [m]"); - title(ax, "Forklift Parking with IPDDP - Frame " + std::to_string(i)); - - // Set grid and axis limits - grid(ax, true); - xlim(ax, {-5, 5}); - ylim(ax, {-5, 5}); - - // Update drawing - fig->draw(); - - // Save the frame to a PNG file - std::string frame_filename = plotDirectory + "/forklift_frame_" + std::to_string(i) + ".png"; - fig->save(frame_filename); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool - std::string command = "convert -delay 10 " + plotDirectory + "/forklift_frame_*.png " - + plotDirectory + "/forklift_parking_ipddp.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/forklift_frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as forklift_parking_ipddp.gif" << std::endl; - - // Print final state - std::cout << "\nFinal state:" << std::endl; - std::cout << "Position: (" << X_sol.back()(0) << ", " << X_sol.back()(1) << ")" << std::endl; - std::cout << "Heading: " << X_sol.back()(2) * 180.0 / M_PI << " degrees" << std::endl; - std::cout << "Velocity: " << X_sol.back()(3) << " m/s" << std::endl; - std::cout << "Steering angle: " << X_sol.back()(4) * 180.0 / M_PI << " degrees" << std::endl; - - return 0; -} \ No newline at end of file diff --git a/examples/cddp_hcw.cpp b/examples/cddp_hcw.cpp deleted file mode 100644 index b9a05085..00000000 --- a/examples/cddp_hcw.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -/* - * Example code demonstrating an HCW (Hill-Clohessy-Wiltshire) rendezvous problem with CDDP - */ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; -using namespace cddp; - -// ---------------------------------------------------------------------------------------- -// Main function demonstrating usage -// ---------------------------------------------------------------------------------------- -int main() { - // Random number generator for optional initial control sequence - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution d(0.0, 0.01); - - // HCW problem dimensions - const int STATE_DIM = 6; // [x, y, z, vx, vy, vz] - const int CONTROL_DIM = 3; // [Fx, Fy, Fz] - - // Set up the time horizon - int horizon = 50; // Number of steps - double timestep = 10.0; // Timestep in seconds (example) - std::string integration_type = "euler"; // or "rk4", etc. - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - - // Create the HCW dynamical system - // (this class is defined in your "spacecraft_linear.hpp") - std::unique_ptr system = - std::make_unique(timestep, mean_motion, mass, integration_type); - - // Initial state (for example, some offset and small velocity) - Eigen::VectorXd initial_state(STATE_DIM); - initial_state << 50.0, 14.0, 0.0, 0.0, 0.0, 0.0; - - // Goal state (origin in relative coordinates, zero velocity) - Eigen::VectorXd goal_state(STATE_DIM); - goal_state.setZero(); - - // Cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - { - Q(0,0) = 1e3; Q(1,1) = 1e3; Q(2,2) = 1e3; - Q(3,3) = 1e1; Q(4,4) = 1e1; Q(5,5) = 1e1; - } - - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - { - R(0,0) = 1e2; R(1,1) = 1e2; R(2,2) = 1e2; - } - - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // { - // Qf(0,0) = 1e3; - // Qf(1,1) = 1e3; - // Qf(2,2) = 1e3; - // Qf(3,3) = 1e1; - // Qf(4,4) = 1e1; - // Qf(5,5) = 1e1; - // } - - std::vector empty_reference_states; - auto objective = std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Set solver options - cddp::CDDPOptions options; - options.max_iterations = 300; // Max number of CDDP iterations - options.verbose = true; // Print progress - options.tolerance = 1e-3; // Stop if improvement below this - options.acceptable_tolerance = 1e-3; // Stop if gradient below this - options.debug = false; - options.enable_parallel = true; - options.num_threads = 8; // Parallelization - - // Create the CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Optionally add control constraints (e.g., max thruster force) - Eigen::VectorXd umin(CONTROL_DIM); - Eigen::VectorXd umax(CONTROL_DIM); - // Suppose each axis force is limited to +/- 2 N - umin << -1.0, -1.0, -1.0; - umax << 1.0, 1.0, 1.0; - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(umin, umax)); - - // Initialize the trajectory (X,U) with something nontrivial - std::vector X(horizon + 1, Eigen::VectorXd::Zero(STATE_DIM)); - std::vector U(horizon, Eigen::VectorXd::Zero(CONTROL_DIM)); - - // Set the initial state - X[0] = initial_state; - - // Random or small constant initialization for control - for (auto& u : U) { - // u << d(gen), d(gen), d(gen); // small random thruster - u << 0.0, 0.0, 0.0; // zero thruster - } - - // Compute the initial cost by rolling out the initial controls - double J_init = 0.0; - for (int t = 0; t < horizon; t++) { - J_init += cddp_solver.getObjective().running_cost(X[t], U[t], t); - X[t+1] = cddp_solver.getSystem().getDiscreteDynamics(X[t], U[t], t * timestep); - } - J_init += cddp_solver.getObjective().terminal_cost(X[horizon]); - std::cout << "[Info] Initial cost: " << J_init << std::endl; - std::cout << "[Info] Initial final state: " << X[horizon].transpose() << std::endl; - - // Pass this initial guess to the solver - cddp_solver.setInitialTrajectory(X, U); - - // Solve - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - - // Extract solution and print result - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::cout << "\n[Result] CDDP solved." << std::endl; - std::cout << "[Result] Final cost: " << solution.final_objective << std::endl; - std::cout << "[Result] Final state: " - << X_sol.back().transpose() << std::endl; - - // Create plot directory - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Extract state data arrays - std::vector x_arr, y_arr, z_arr, vx_arr, vy_arr, vz_arr, time_arr; - for (size_t i = 0; i < X_sol.size(); ++i) { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - vx_arr.push_back(X_sol[i](3)); - vy_arr.push_back(X_sol[i](4)); - vz_arr.push_back(X_sol[i](5)); - } - - // Extract control data arrays (note: U_sol size is horizon, so use t_sol[i] for control time) - std::vector u1_arr, u2_arr, u3_arr, t_control; - for (size_t i = 0; i < U_sol.size(); ++i) { - u1_arr.push_back(U_sol[i](0)); - u2_arr.push_back(U_sol[i](1)); - u3_arr.push_back(U_sol[i](2)); - t_control.push_back(t_sol[i]); // assign control time to corresponding state time step - } - - // ------------------------------- - // Plot state history (position & velocity) - // ------------------------------- - auto fig1 = figure(true); - - // Position subplot - subplot(2,1,1); - plot(time_arr, x_arr, "-o")->line_width(2); - hold(on); - plot(time_arr, y_arr, "-o")->line_width(2); - plot(time_arr, z_arr, "-o")->line_width(2); - title("Position vs. Time"); - xlabel("Time [s]"); - ylabel("Position"); - matplot::legend({"x", "y", "z"}); - - // Velocity subplot - subplot(2,1,2); - plot(time_arr, vx_arr, "-o")->line_width(2); - hold(on); - plot(time_arr, vy_arr, "-o")->line_width(2); - plot(time_arr, vz_arr, "-o")->line_width(2); - title("Velocity vs. Time"); - xlabel("Time [s]"); - ylabel("Velocity"); - matplot::legend({"vx", "vy", "vz"}); - - // ------------------------------- - // Plot control history - // ------------------------------- - auto fig2 = figure(true); - plot(t_control, u1_arr, "-o")->line_width(2); - hold(on); - plot(t_control, u2_arr, "-o")->line_width(2); - plot(t_control, u3_arr, "-o")->line_width(2); - title("Control Inputs vs. Time"); - xlabel("Time [s]"); - ylabel("Control Input"); - matplot::legend({"u1", "u2", "u3"}); - - // Optionally save the figures - save(fig1, plotDirectory + "/hcw_state_history.png"); - save(fig2, plotDirectory + "/hcw_control_history.png"); - return 0; -} diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp deleted file mode 100644 index 7844579a..00000000 --- a/examples/cddp_lti_system.cpp +++ /dev/null @@ -1,172 +0,0 @@ -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -#include "matplot/matplot.h" -using namespace matplot; -namespace fs = std::filesystem; - -namespace cddp { - -/** - * @brief Quadratic cost objective for LTI system - */ -class LTIObjective : public QuadraticObjective { -public: - LTIObjective(int state_dim, - int control_dim, - const Eigen::VectorXd& goal_state, - double timestep) - : QuadraticObjective( - 0.5 * timestep * Eigen::MatrixXd::Identity(state_dim, state_dim), // Q - 0.5 * 0.1 * timestep * Eigen::MatrixXd::Identity(control_dim, control_dim), // R - 0.5 * timestep * Eigen::MatrixXd::Identity(state_dim, state_dim), // Qf - goal_state) {} -}; - -} // namespace cddp - -template -void printVector(const std::string& label, const std::vector& vec) { - std::cout << label << ": ["; - for (size_t i = 0; i < vec.size(); ++i) { - if (i > 0) std::cout << ", "; - std::cout << vec[i]; - } - std::cout << "]\n"; -} - -void plotStateTrajectories(const std::vector& X_sol, - const std::string& plotDirectory) { - const int state_dim = X_sol[0].size(); - std::vector> state_histories(state_dim); - std::vector time_history; - - // Extract state histories (using the time index as sample number) - for (size_t t = 0; t < X_sol.size(); t++) { - time_history.push_back(static_cast(t)); - for (int i = 0; i < state_dim; i++) { - state_histories[i].push_back(X_sol[t](i)); - } - } - - // Create a new figure and set its size - auto fig = figure(true); - fig->size(1200, 800); - - // Create a subplot for each state variable - for (int i = 0; i < state_dim; i++) { - // Create subplot: (rows, columns, index) - subplot(state_dim, 1, i + 1); - plot(time_history, state_histories[i])->line_width(2); - title("State " + std::to_string(i + 1)); - grid(on); - } - - save(fig, plotDirectory + "/lti_states.png"); -} - -int main() { - // Problem parameters - int state_dim = 4; // State dimension - int control_dim = 2; // Control dimension - int horizon = 1000; // Time horizon - double timestep = 0.01; // Time step - std::string integration_type = "euler"; - - // Create LTI system instance - std::unique_ptr system = - std::make_unique(state_dim, control_dim, timestep, integration_type); - - // Initial and goal states - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state << 0.8378, 0.3794, 1.4796, 0.2382; - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - - // Create cost objective - Eigen::MatrixXd Q = 0.5 * Eigen::MatrixXd::Identity(state_dim, state_dim); // Q - Eigen::MatrixXd R = 0.5 * 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); // R - Eigen::MatrixXd Qf = 0.5 * timestep * Eigen::MatrixXd::Identity(state_dim, state_dim); // Qf - std::vector empty_reference_states; - auto objective = std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 10; - options.verbose = true; - options.regularization.initial_value = 0.0; - options.num_threads = 11; - options.enable_parallel = true; - options.debug = false; - - // Create CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Control constraints - Eigen::VectorXd control_lower_bound = -0.6 * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_upper_bound = 0.6 * Eigen::VectorXd::Ones(control_dim); - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique( - control_lower_bound, control_upper_bound)); - - // Initialize trajectories - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Random initial controls (here, a constant small control is used) - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution d(0.0, 0.1); - for (auto& u : U) { - u = 0.01 * Eigen::VectorXd::Ones(control_dim); - } - - X[0] = initial_state; - - // Simulate initial trajectory - double J = 0.0; - double cost = 0.0; - for (size_t t = 0; t < horizon; t++) { - cost = cddp_solver.getObjective().running_cost(X[t], U[t], t); - J += cost; - X[t + 1] = cddp_solver.getSystem().getDiscreteDynamics(X[t], U[t], t * timestep); - } - J += cddp_solver.getObjective().terminal_cost(X.back()); - std::cout << "Initial state: " << X[0].transpose() << std::endl; - std::cout << "Final state: " << X.back().transpose() << std::endl; - std::cout << "Initial cost: " << J << std::endl; - - cddp_solver.setInitialTrajectory(X, U); - - // Solve using the CDDP solver - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - // Alternatively: cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::LogDDP); - - // Extract solution trajectories - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - auto iterations = solution.iterations_completed; - bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound"); - - // Create directory for plots if it doesn't exist - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Plot state trajectories using the matplot API - plotStateTrajectories(X_sol, plotDirectory); - - // Print optimization statistics - std::cout << "\nOptimization Results:" << std::endl; - std::cout << "Iterations: " << iterations << std::endl; - std::cout << "Final cost: " << solution.final_objective << std::endl; - std::cout << "Converged: " << (converged ? "Yes" : "No") << std::endl; - std::cout << "Solve time: " << solution.solve_time_ms << " ms" << std::endl; - - return 0; -} diff --git a/examples/cddp_manipulator.cpp b/examples/cddp_manipulator.cpp index 24278b5f..3ad8b8eb 100644 --- a/examples/cddp_manipulator.cpp +++ b/examples/cddp_manipulator.cpp @@ -13,232 +13,70 @@ See the License for the specific language governing permissions and limitations under the License. */ + #include #include -#include -#include -#include -#include -#include // for sleep_for #include "cddp.hpp" -#include "cddp_example_utils.hpp" - -#include "matplot/matplot.h" -using namespace matplot; -namespace fs = std::filesystem; int main() { - // -------------------- System and Problem Setup -------------------- - // System dimensions: state = [q1, q2, q3, dq1, dq2, dq3], control = [tau1, tau2, tau3] - int state_dim = 6; - int control_dim = 3; - int horizon = 200; // Time horizon for optimization - double timestep = 0.01; + constexpr double kPi = 3.14159265358979323846; + constexpr int state_dim = 6; + constexpr int control_dim = 3; + constexpr int horizon = 160; + constexpr double timestep = 0.01; + + Eigen::VectorXd initial_state(state_dim); + initial_state << 0.0, -kPi / 2.0, kPi, 0.0, 0.0, 0.0; - // Create a manipulator instance (assumed to be defined in your CDDP framework) - auto system = std::make_unique(timestep, "rk4"); + Eigen::VectorXd goal_state(state_dim); + goal_state << kPi, -kPi / 6.0, -kPi / 3.0, 0.0, 0.0, 0.0; - // Cost matrices Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - // Cost on joint angles Q.diagonal().segment(0, 3) = Eigen::Vector3d::Ones(); - // Cost on joint velocities Q.diagonal().segment(3, 3) = 0.1 * Eigen::Vector3d::Ones(); - Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = 100.0 * Q; // Terminal cost - - // Initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, -M_PI/2, M_PI, 0.0, 0.0, 0.0; - Eigen::VectorXd goal_state(state_dim); - goal_state << M_PI, -M_PI/6, -M_PI/3, 0.0, 0.0, 0.0; + Eigen::MatrixXd Qf = 100.0 * Q; - // Create quadratic objective (with no reference trajectory) - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Solver options cddp::CDDPOptions options; - options.max_iterations = 100; + options.max_iterations = 80; options.line_search.max_iterations = 20; - options.verbose = true; - - // Create and configure the CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - // Control constraints: joint torques limited to ±max_torque - double max_torque = 50.0; - Eigen::VectorXd control_lower_bound = -max_torque * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_upper_bound = max_torque * Eigen::VectorXd::Ones(control_dim); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); + cddp::CDDP solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique(timestep, "rk4"), + std::make_unique( + Q, R, Qf, goal_state, std::vector{}, timestep), + options); + + const Eigen::VectorXd lower = + -50.0 * Eigen::VectorXd::Ones(control_dim); + const Eigen::VectorXd upper = + 50.0 * Eigen::VectorXd::Ones(control_dim); + solver.addPathConstraint("ControlConstraint", + std::make_unique(lower, upper)); - // Initialize trajectories: here we use linear interpolation between initial and goal state std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); for (int i = 0; i <= horizon; ++i) { - double alpha = static_cast(i) / horizon; + const double alpha = static_cast(i) / static_cast(horizon); X[i] = (1.0 - alpha) * initial_state + alpha * goal_state; } - cddp_solver.setInitialTrajectory(X, U); - - // Solve the optimal control problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - - // -------------------- Extract Trajectories for Static Plots -------------------- - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::vector time; // for state trajectories - std::vector time_ctrl; // for control trajectories - // For joint angles and velocities (each has 3 joints) - std::vector> joint_angles(3), joint_velocities(3); - std::vector> joint_torques(3); - - // Loop over the time sequence to extract states and controls - for (size_t i = 0; i < t_sol.size(); ++i) { - time.push_back(t_sol[i]); - if (i < X_sol.size()) { - for (int j = 0; j < 3; ++j) { - joint_angles[j].push_back(X_sol[i](j)); - joint_velocities[j].push_back(X_sol[i](j + 3)); - } - } - if (i < U_sol.size()) { - for (int j = 0; j < 3; ++j) { - joint_torques[j].push_back(U_sol[i](j)); - } - time_ctrl.push_back(t_sol[i]); - } - } - - // -------------------- Static Plot: Joint Angles, Velocities, and Torques -------------------- - auto fig1 = figure(true); - fig1->size(1200, 800); - - // Joint angles subplot - auto ax1_fig1 = subplot(3, 1, 1); - for (int j = 0; j < 3; ++j) { - auto plot_handle = plot(ax1_fig1, time, joint_angles[j], "-o"); - plot_handle->line_width(2); - plot_handle->display_name("Joint " + std::to_string(j + 1)); - } - title("Joint Angles"); - xlabel("Time [s]"); - ylabel("Angle [rad]"); - matplot::legend(ax1_fig1); - grid(on); - - // Joint velocities subplot - auto ax2_fig1 = subplot(3, 1, 2); - for (int j = 0; j < 3; ++j) { - auto plot_handle = plot(ax2_fig1, time, joint_velocities[j], "-o"); - plot_handle->line_width(2); - plot_handle->display_name("Joint " + std::to_string(j + 1)); - } - title("Joint Velocities"); - xlabel("Time [s]"); - ylabel("Velocity [rad/s]"); - matplot::legend(ax2_fig1); - grid(on); - - // Joint torques subplot - auto ax3_fig1 = subplot(3, 1, 3); - for (int j = 0; j < 3; ++j) { - auto plot_handle = plot(ax3_fig1, time_ctrl, joint_torques[j], "-o"); - plot_handle->line_width(2); - plot_handle->display_name("Joint " + std::to_string(j + 1)); - } - title("Joint Torques"); - xlabel("Time [s]"); - ylabel("Torque [Nm]"); - matplot::legend(ax3_fig1); - grid(on); - - // Create a directory for plots if it doesn't exist - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - save(fig1, plotDirectory + "/manipulator_cddp_results.png"); - - // Create a new figure for animation - auto fig2 = figure(true); - fig2->size(800, 600); - auto ax2_fig2 = fig2->current_axes(); - cddp::Manipulator manipulator(timestep, "rk4"); - - // Animate every 5th frame of the solution - for (size_t i = 0; i < X_sol.size(); i += 5) { - ax2_fig2->clear(); // clear current figure - ax2_fig2->hold(true); // hold the current plot - ax2_fig2->grid(true); // enable grid - - const auto& state = X_sol[i]; - - auto transforms = manipulator.getTransformationMatrices(state(0), state(1), state(2)); - Eigen::Matrix4d T01 = transforms[0]; - Eigen::Matrix4d T02 = T01 * transforms[1]; - Eigen::Matrix4d T03 = T02 * transforms[2]; - Eigen::Matrix4d T04 = T03 * transforms[3]; - - // Compute end-effector position. - Eigen::Vector4d r3; - r3 << manipulator.getLinkLength('c'), 0, manipulator.getLinkLength('b'), 1; - Eigen::Vector4d r0 = T03 * r3; // End-effector position in base frame - // Compute an intermediate (elbow) position - Eigen::Vector4d rm = T03 * Eigen::Vector4d(0, 0, manipulator.getLinkLength('b'), 1); - - // Prepare data for plotting: - std::vector x = {0, T03(0, 3), rm(0), r0(0)}; - std::vector y = {0, T03(1, 3), rm(1), r0(1)}; - std::vector z = {0, T03(2, 3), rm(2), r0(2)}; - auto link_line = plot3(x, y, z, "-o"); - link_line->line_width(2); - link_line->color("blue"); - - // Plot joints (using markers only) - std::vector joint_x = {0, T03(0, 3), rm(0)}; - std::vector joint_y = {0, T03(1, 3), rm(1)}; - std::vector joint_z = {0, T03(2, 3), rm(2)}; - auto joint_markers = plot3(joint_x, joint_y, joint_z, "o"); - joint_markers->color("red"); - - // Plot end-effector as a marker - std::vector ee_x = {r0(0)}; - std::vector ee_y = {r0(1)}; - std::vector ee_z = {r0(2)}; - auto ee_marker = plot3(ee_x, ee_y, ee_z, "o"); - ee_marker->color("red"); - - xlabel("X [m]"); - ylabel("Y [m]"); - zlabel("Z [m]"); - title("Manipulator CDDP Solution"); - xlim({-2, 2}); - ylim({-2, 2}); - zlim({-1, 3}); - view(30, -60); - - // Save each frame (e.g., "manipulator_frame_0.png", "manipulator_frame_1.png", …) - std::string filename = plotDirectory + "/manipulator_frame_" + std::to_string(i/5) + ".png"; - save(fig2, filename); + solver.setInitialTrajectory(X, U); - // Pause for 10 milliseconds between frames - std::this_thread::sleep_for(std::chrono::milliseconds(40)); + const cddp::CDDPSolution solution = solver.solve(cddp::SolverType::CLDDP); + if (solution.state_trajectory.empty()) { + std::cerr << "Manipulator example failed: empty trajectory" << std::endl; + return 1; } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 20 " + plotDirectory + "/manipulator_frame_*.png " + plotDirectory + "/manipulator.gif"; - std::system(gif_command.c_str()); - std::string cleanup_command = "rm " + plotDirectory + "/manipulator_frame_*.png"; - std::system(cleanup_command.c_str()); - + const Eigen::VectorXd final_error = solution.state_trajectory.back() - goal_state; + std::cout << "Manipulator example completed with status: " + << solution.status_message << '\n' + << "Final objective: " << solution.final_objective << '\n' + << "Final state error norm: " << final_error.norm() << std::endl; return 0; } diff --git a/examples/cddp_pendulum.cpp b/examples/cddp_pendulum.cpp index 19654f63..6972fb37 100644 --- a/examples/cddp_pendulum.cpp +++ b/examples/cddp_pendulum.cpp @@ -13,232 +13,67 @@ See the License for the specific language governing permissions and limitations under the License. */ + #include #include -#include -#include -#include -#include -#include + #include "cddp.hpp" #include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; int main() { - // -------------------- Problem Setup -------------------- - int state_dim = 2; - int control_dim = 1; - int horizon = 100; - double timestep = 0.02; - - // Create a pendulum instance - double mass = 1.0; - double length = 0.5; - double damping = 0.01; - std::string integration_type = "euler"; - - // Cost matrices + constexpr double kPi = 3.14159265358979323846; + constexpr int state_dim = 2; + constexpr int control_dim = 1; + constexpr int horizon = 100; + constexpr double timestep = 0.02; + + Eigen::VectorXd initial_state(state_dim); + initial_state << kPi, 0.0; + + Eigen::VectorXd goal_state(state_dim); + goal_state << 0.0, 0.0; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 100.0, 0.0, - 0.0, 100.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0; // Upright position with zero velocity - - std::vector empty_reference_states; - - // Initial state (pendulum pointing down) - Eigen::VectorXd initial_state(state_dim); - initial_state << M_PI, 0.0; - - // Construct a zero control sequence and an initial trajectory (all at the initial state) - std::vector zero_control_sequence(horizon, Eigen::VectorXd::Zero(control_dim)); - std::vector X_init(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - for (int t = 0; t < horizon + 1; ++t) { - X_init[t] = initial_state; - } - - // (Optional) Calculate initial cost - auto temp_objective = std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep); - double J = 0.0; - for (int t = 0; t < horizon; ++t) { - J += temp_objective->running_cost(X_init[t], zero_control_sequence[t], t); - } - J += temp_objective->terminal_cost(X_init[horizon]); - - // Control constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -100.0; - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 100.0; - - // Solver options + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + cddp::CDDPOptions options; - options.max_iterations = 20; + options.max_iterations = 30; options.tolerance = 1e-4; options.acceptable_tolerance = 1e-5; - options.regularization.initial_value = 1e-7; - - // Create and configure the CDDP solver with new API - cddp::CDDP cddp_solver( + options.regularization.initial_value = 1e-6; + + cddp::CDDP solver( initial_state, goal_state, horizon, timestep, - std::make_unique(timestep, length, mass, damping, integration_type), - std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - - // Set initial trajectory for the solver - auto [X, U] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); - cddp_solver.setInitialTrajectory(X, U); - - // Solve the optimal control problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - - // Create plot directory if it doesn't exist - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // -------------------- Data Extraction -------------------- - auto theta_arr = cddp::example::extractComponent(X_sol, 0); - auto theta_dot_arr = cddp::example::extractComponent(X_sol, 1); - auto torque_arr = cddp::example::extractComponent(U_sol, 0); - - // Build time vectors (state has horizon+1 points; control has horizon points) - auto time_state = cddp::example::makeTimeVector(X_sol.size(), timestep); - auto time_control = cddp::example::makeTimeVector(U_sol.size(), timestep); - - // -------------------- Static Plot -------------------- - auto fig1 = figure(true); - fig1->size(1200, 800); - - // Subplot for state trajectory (angle and angular velocity) - auto ax1 = subplot(2, 1, 0); - { - ax1->hold(true); - auto plot_handle1 = plot(ax1, time_state, theta_arr, "-o"); - plot_handle1->display_name("Angle"); - plot_handle1->line_width(2); - plot_handle1->color("b"); - - auto plot_handle2 = plot(ax1, time_state, theta_dot_arr, "-o"); - plot_handle2->display_name("Angular Velocity"); - plot_handle2->line_width(2); - plot_handle2->color("r"); - ax1->title("Pendulum State Trajectory"); - ax1->xlabel("Time [s]"); - ax1->ylabel("Value"); - ax1->legend(); - ax1->grid(true); - - } - // Subplot for control input (torque) - auto ax2 = subplot(2, 1, 1); - { - ax2->hold(true); - auto plot_handle3 = plot(ax2, time_control, torque_arr, "-o"); - plot_handle3->display_name("Torque"); - plot_handle3->line_width(2); - plot_handle3->color("g"); - - // Customize the plot - ax2->title("Control Input"); - ax2->xlabel("Time [s]"); - ax2->ylabel("Torque [Nm]"); - ax2->legend(); - ax2->grid(true); - } - - // Adjust layout if supported - // tight_layout(); // Uncomment if your matplot version supports it - save(fig1, plotDirectory + "/pendulum_cddp_test.png"); - - // --- Animation --- - auto fig3 = figure(); - auto ax_fig3 = fig3->current_axes(); - fig3->size(800, 600); - title("Pendulum Animation"); - double pendulum_length = length; - double bob_radius = 0.1; + std::make_unique(timestep, 0.5, 1.0, 0.01, "euler"), + std::make_unique( + Q, R, Qf, goal_state, std::vector{}, timestep), + options); + Eigen::VectorXd lower(control_dim); + lower << -20.0; + Eigen::VectorXd upper(control_dim); + upper << 20.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(lower, upper)); - for (int i = 0; i < X_sol.size(); ++i) { - // Animate every 5th frame - if (i % 5 == 0) { - // Clear previous content. - cla(ax_fig3); - hold(ax_fig3, true); - - double theta = theta_arr[i]; - // Calculate pendulum bob position (x, y) - double x = pendulum_length * std::sin(theta); - double y = pendulum_length * std::cos(theta); - - // Plot pendulum rod - std::vector rod_x = {0, x}; - std::vector rod_y = {0, y}; - auto rod_plot = plot(rod_x, rod_y); - rod_plot->line_style("k-"); - rod_plot->line_width(2); + auto [X, U] = + cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); + solver.setInitialTrajectory(X, U); - - // Plot pendulum bob as a circle - std::vector circle_x, circle_y; - int num_points = 50; - for (int j = 0; j <= num_points; ++j) { - double t = 2 * M_PI * j / num_points; - circle_x.push_back(x + bob_radius * std::cos(t)); - circle_y.push_back(y + bob_radius * std::sin(t)); - } - auto circle_plot = plot(circle_x, circle_y); - circle_plot->line_style("b-"); - circle_plot->line_width(2); - - - // Plot trajectory trace for the last 50 frames, if available - std::vector trace_x, trace_y; - int start = std::max(0, i - 50); - for (int j = start; j < i; ++j) { - trace_x.push_back(pendulum_length * std::sin(theta_arr[j])); - trace_y.push_back(pendulum_length * std::cos(theta_arr[j])); - } - if (!trace_x.empty()) { - auto trace_plot = plot(trace_x, trace_y); - trace_plot->line_style("r--"); - trace_plot->line_width(1); - } - // Set fixed axis limits for stable animation. - ax_fig3->xlim({-0.7, 0.7}); - ax_fig3->ylim({-0.7, 0.7}); - - // Save the current frame as an image file - std::string filename = plotDirectory + "/pendulum_frame_" + std::to_string(i) + ".png"; - save(fig3, filename); - - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - } + const cddp::CDDPSolution solution = solver.solve(cddp::SolverType::IPDDP); + if (solution.state_trajectory.empty()) { + std::cerr << "Pendulum example failed: empty trajectory" << std::endl; + return 1; } - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 30 " + plotDirectory + "/pendulum_frame_*.png " + plotDirectory + "/pendulum.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/pendulum_frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as pendulum.gif" << std::endl; - + const Eigen::VectorXd final_error = solution.state_trajectory.back() - goal_state; + std::cout << "Pendulum example completed with status: " + << solution.status_message << '\n' + << "Final objective: " << solution.final_objective << '\n' + << "Final state error norm: " << final_error.norm() << std::endl; return 0; } diff --git a/examples/cddp_quadrotor_circle.cpp b/examples/cddp_quadrotor_circle.cpp deleted file mode 100644 index 22f7ec0b..00000000 --- a/examples/cddp_quadrotor_circle.cpp +++ /dev/null @@ -1,566 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -// Helper function: Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Helper function: Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - - return R; -} - -// Helper: Transform quadrotor frame points (motor positions) to world coordinates using quaternion. -std::vector> transformQuadrotorFrame( - const Eigen::Vector3d &position, - const Eigen::Vector4d &quat, // [qw, qx, qy, qz] - double arm_length) -{ - - // Define quadrotor motor positions in the body frame - std::vector body_points = { - Eigen::Vector3d(arm_length, 0, 0), // Front motor - Eigen::Vector3d(0, arm_length, 0), // Right motor - Eigen::Vector3d(-arm_length, 0, 0), // Back motor - Eigen::Vector3d(0, -arm_length, 0) // Left motor - }; - - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat[0], quat[1], quat[2], quat[3]); - - std::vector> world_points(3, std::vector()); - for (const auto &pt : body_points) - { - Eigen::Vector3d wp = position + R * pt; - world_points[0].push_back(wp.x()); - world_points[1].push_back(wp.y()); - world_points[2].push_back(wp.z()); - } - return world_points; -} - -int main() -{ - // For quaternion-based quadrotor, state_dim = 13: - // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - int state_dim = 13; - int control_dim = 4; // [f1, f2, f3, f4] - int horizon = 400; // Longer horizon for 3D maneuvers - double timestep = 0.02; - - // Quadrotor parameters - double mass = 1.0; // 1kg quadrotor - double arm_length = 0.2; // 20cm arm length - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 0.01; // Ixx - inertia_matrix(1, 1) = 0.01; // Iyy - inertia_matrix(2, 2) = 0.02; // Izz - - std::string integration_type = "rk4"; - - // Create the dynamical system (quadrotor) as a unique_ptr - std::unique_ptr system = - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type); - - // For propagation - cddp::Quadrotor quadrotor(timestep, mass, inertia_matrix, arm_length, integration_type); - - // Cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Q(0, 0) = 1.0; - Q(1, 1) = 1.0; - Q(2, 2) = 1.0; - Q(3, 3) = 1.0; - Q(4, 4) = 1.0; - Q(5, 5) = 1.0; - Q(6, 6) = 1.0; - - // Control cost matrix (penalize aggressive control inputs) - Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - // Terminal cost matrix (important for stability) - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 1.0; - Qf(1, 1) = 1.0; - Qf(2, 2) = 1.0; - Qf(3, 3) = 1.0; - Qf(4, 4) = 1.0; - Qf(5, 5) = 1.0; - Qf(6, 6) = 1.0; - - // Parameters for the circular trajectory - double circle_radius = 3.0; // e.g., 3m radius - Eigen::Vector2d circle_center(0.0, 0.0); // center of the circle in the x-y plane - double constant_altitude = 2.0; // fixed altitude (z) - double total_time = horizon * timestep; // total duration - // omega is chosen so that the quadrotor completes one full circle over the time horizon - double omega = 2 * M_PI / total_time; - - std::vector circle_reference_states; - circle_reference_states.reserve(horizon + 1); - - for (int i = 0; i <= horizon; ++i) - { - double t = i * timestep; - double angle = omega * t; - - // Create a reference state of dimension state_dim (13) - Eigen::VectorXd ref_state = Eigen::VectorXd::Zero(state_dim); - - // Position (x, y, z) - ref_state(0) = circle_center(0) + circle_radius * std::cos(angle); - ref_state(1) = circle_center(1) + circle_radius * std::sin(angle); - ref_state(2) = constant_altitude; - - // Orientation: set to identity quaternion [1, 0, 0, 0] - ref_state(3) = 1.0; // qw - ref_state(4) = 0.0; // qx - ref_state(5) = 0.0; // qy - ref_state(6) = 0.0; // qz // z velocity - - circle_reference_states.push_back(ref_state); - } - - // Goal state: hover at position (3,0,2) with identity quaternion and zero velocities. - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state(0) = circle_center(0) + circle_radius; - goal_state(2) = constant_altitude; - goal_state(3) = 1.0; // Identity quaternion: qw = 1 - - auto objective = std::make_unique( - Q, R, Qf, goal_state, circle_reference_states, timestep); - - // Initial state (at origin with identity quaternion) - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state(0) = circle_center(0) + circle_radius; - initial_state(2) = constant_altitude; - initial_state(3) = 1.0; // Identity quaternion: qw = 1 - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 10000; - options.verbose = true; - options.debug = false; - options.enable_parallel = true; - options.num_threads = 10; - options.tolerance = 1e-3; - options.regularization.initial_value = 1e-4; - options.ipddp.barrier.mu_initial = 1e-1; - options.msipddp.segment_length = horizon / 10; - options.msipddp.rollout_type = "nonlinear"; - options.ipddp.barrier.mu_update_factor = 0.2; - options.ipddp.barrier.mu_update_power = 1.2; - - // Create the CDDP solver - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::move(system), - std::move(objective), - options); - - // Control constraints (motor thrust limits) - double min_force = 0.0; // Motors can only produce thrust upward - double max_force = 4.0; // Maximum thrust per motor - Eigen::VectorXd control_upper_bound = max_force * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_lower_bound = min_force * Eigen::VectorXd::Ones(control_dim); - // cddp_solver.addConstraint("ControlConstraint", - // std::make_unique(control_lower_bound, control_upper_bound)); - - // Initial trajectory: allocate state and control trajectories - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Initialize with hovering thrust (each motor provides mg/4) - double hover_thrust = mass * 9.81 / 4.0; - for (auto &u : U) - { - u = hover_thrust * Eigen::VectorXd::Ones(control_dim); - } - // Propagate initial trajectory using discrete dynamics - // for (size_t i = 0; i < static_cast(horizon); ++i) - // { - // X[i + 1] = quadrotor.getDiscreteDynamics(X[i], U[i]); - // } - - // Initialize state by the actual reference trajectory - X = circle_reference_states; - cddp_solver.setInitialTrajectory(X, U); - - // Solve the optimal control problem (LogDDP, IPDDP, MSIPDDP) - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::cout << "Final state: " << X_sol.back().transpose() << std::endl; - - // // Create plot directory if it doesn't exist - // const std::string plotDirectory = "../results/tests"; - // if (!fs::exists(plotDirectory)) - // { - // fs::create_directory(plotDirectory); - // } - // // Create a directory for frame images. - // (void)std::system("mkdir -p frames"); - - // // Extract trajectory data - // std::vector time_arr, x_arr, y_arr, z_arr; - // std::vector phi_arr, theta_arr, psi_arr; - // std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - // time_arr.reserve(X_sol.size()); - // x_arr.reserve(X_sol.size()); - // y_arr.reserve(X_sol.size()); - // z_arr.reserve(X_sol.size()); - // phi_arr.reserve(X_sol.size()); - // theta_arr.reserve(X_sol.size()); - // psi_arr.reserve(X_sol.size()); - - // qw_arr.reserve(X_sol.size()); - // qx_arr.reserve(X_sol.size()); - // qy_arr.reserve(X_sol.size()); - // qz_arr.reserve(X_sol.size()); - // q_norm_arr.reserve(X_sol.size()); - - // for (size_t i = 0; i < X_sol.size(); ++i) - // { - // time_arr.push_back(t_sol[i]); - // x_arr.push_back(X_sol[i](0)); - // y_arr.push_back(X_sol[i](1)); - // z_arr.push_back(X_sol[i](2)); - - // double qw = X_sol[i](3); - // double qx = X_sol[i](4); - // double qy = X_sol[i](5); - // double qz = X_sol[i](6); - - // // Euler angles - // Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - // phi_arr.push_back(euler(0)); - // theta_arr.push_back(euler(1)); - // psi_arr.push_back(euler(2)); - - // // Quaternion data - // qw_arr.push_back(qw); - // qx_arr.push_back(qx); - // qy_arr.push_back(qy); - // qz_arr.push_back(qz); - // q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - // } - - // // Control data - // std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - // std::vector f1_arr, f2_arr, f3_arr, f4_arr; - // f1_arr.reserve(U_sol.size()); - // f2_arr.reserve(U_sol.size()); - // f3_arr.reserve(U_sol.size()); - // f4_arr.reserve(U_sol.size()); - - // for (const auto &u : U_sol) - // { - // f1_arr.push_back(u(0)); - // f2_arr.push_back(u(1)); - // f3_arr.push_back(u(2)); - // f4_arr.push_back(u(3)); - // } - - // // Plotting - // auto f1 = figure(); - // f1->size(1200, 900); // Slightly bigger if you like - - // // (1) Position - // auto ax1_f1 = subplot(4, 1, 0); - // auto plot_handle1 = plot(ax1_f1, time_arr, x_arr, "-r"); - // plot_handle1->display_name("x"); - // plot_handle1->line_width(2); - // hold(ax1_f1, true); - // auto plot_handle2 = plot(ax1_f1, time_arr, y_arr, "-g"); - // plot_handle2->display_name("y"); - // plot_handle2->line_width(2); - // auto plot_handle3 = plot(ax1_f1, time_arr, z_arr, "-b"); - // plot_handle3->display_name("z"); - // plot_handle3->line_width(2); - - // title(ax1_f1, "Position Trajectories"); - // xlabel(ax1_f1, "Time [s]"); - // ylabel(ax1_f1, "Position [m]"); - // matplot::legend(ax1_f1); - // grid(ax1_f1, true); - - // // (2) Attitude (Euler angles) - // auto ax2_f1 = subplot(4, 1, 1); - // hold(ax2_f1, true); - // auto plot_handle4 = plot(ax2_f1, time_arr, phi_arr, "-r"); - // plot_handle4->display_name("roll"); - // plot_handle4->line_width(2); - // auto plot_handle5 = plot(ax2_f1, time_arr, theta_arr, "-g"); - // plot_handle5->display_name("pitch"); - // plot_handle5->line_width(2); - // auto plot_handle6 = plot(ax2_f1, time_arr, psi_arr, "-b"); - // plot_handle6->display_name("yaw"); - // plot_handle6->line_width(2); - - // title(ax2_f1, "Attitude Angles"); - // xlabel(ax2_f1, "Time [s]"); - // ylabel(ax2_f1, "Angle [rad]"); - // matplot::legend(ax2_f1); - // grid(ax2_f1, true); - - // // (3) Motor forces - // auto ax3_f1 = subplot(4, 1, 2); - // auto plot_handle7 = plot(ax3_f1, time_arr2, f1_arr, "-r"); - // plot_handle7->display_name("f1"); - // plot_handle7->line_width(2); - // hold(ax3_f1, true); - // auto plot_handle8 = plot(ax3_f1, time_arr2, f2_arr, "-g"); - // plot_handle8->display_name("f2"); - // plot_handle8->line_width(2); - // auto plot_handle9 = plot(ax3_f1, time_arr2, f3_arr, "-b"); - // plot_handle9->display_name("f3"); - // plot_handle9->line_width(2); - // auto plot_handle10 = plot(ax3_f1, time_arr2, f4_arr, "-k"); - // plot_handle10->display_name("f4"); - // plot_handle10->line_width(2); - - // title(ax3_f1, "Motor Forces"); - // xlabel(ax3_f1, "Time [s]"); - // ylabel(ax3_f1, "Force [N]"); - // matplot::legend(ax3_f1); - // grid(ax3_f1, true); - - // // (4) Quaternion trajectories and norm - // auto ax4_f1 = subplot(4, 1, 3); - // hold(ax4_f1, true); - - // auto qwh = plot(ax4_f1, time_arr, qw_arr, "-r"); - // qwh->display_name("q_w"); - // qwh->line_width(2); - - // auto qxh = plot(ax4_f1, time_arr, qx_arr, "-g"); - // qxh->display_name("q_x"); - // qxh->line_width(2); - - // auto qyh = plot(ax4_f1, time_arr, qy_arr, "-b"); - // qyh->display_name("q_y"); - // qyh->line_width(2); - - // auto qzh = plot(ax4_f1, time_arr, qz_arr, "-m"); - // qzh->display_name("q_z"); - // qzh->line_width(2); - - // // Norm in black - // auto qnorm_handle = plot(ax4_f1, time_arr, q_norm_arr, "-k"); - // qnorm_handle->display_name("|q|"); - - // title(ax4_f1, "Quaternion Components and Norm"); - // xlabel(ax4_f1, "Time [s]"); - // ylabel(ax4_f1, "Value"); - // matplot::legend(ax4_f1); - // grid(ax4_f1, true); - - // // Save figure 1 - // f1->draw(); - // f1->save(plotDirectory + "/quadrotor_circle_states.png"); - - // // 3D Trajectory - // auto f2 = figure(); - // f2->size(800, 600); - // auto ax2 = f2->current_axes(); - // hold(ax2, true); - // auto traj3d = plot3(ax2, x_arr, y_arr, z_arr); - // traj3d->display_name("Trajectory"); - // traj3d->line_style("-"); - // traj3d->line_width(2); - // traj3d->color("blue"); - // // Project trajectory onto x-y plane at z=0 - // auto proj_xy = plot3(ax2, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - // proj_xy->display_name("X-Y Projection"); - // proj_xy->line_style("--"); - // proj_xy->line_width(1); - // proj_xy->color("gray"); - // xlabel(ax2, "X [m]"); - // ylabel(ax2, "Y [m]"); - // zlabel(ax2, "Z [m]"); - // xlim(ax2, {-5, 5}); - // ylim(ax2, {-5, 5}); - // zlim(ax2, {0, 5}); - // title(ax2, "3D Trajectory (Figure-8)"); - // grid(ax2, true); - // f2->draw(); - // f2->save(plotDirectory + "/quadrotor_circle_3d.png"); - - // // Animation of the quadrotor frame - // auto f_anim = figure(); - // f_anim->size(800, 600); - // auto ax_anim = f_anim->current_axes(); - - // // For collecting the trajectory as we go - // std::vector anim_x, anim_y, anim_z; - // anim_x.reserve(X_sol.size()); - // anim_y.reserve(X_sol.size()); - // anim_z.reserve(X_sol.size()); - - // // Render every Nth frame to reduce #images - // int frame_stride = 15; - // double prop_radius = 0.03; // radius for small spheres at motor ends - - // for (size_t i = 0; i < X_sol.size(); i += frame_stride) - // { - // ax_anim->clear(); - // ax_anim->hold(true); - // ax_anim->grid(true); - - // // Current state - // double x = X_sol[i](0); - // double y = X_sol[i](1); - // double z = X_sol[i](2); - - // // Accumulate path - // anim_x.push_back(x); - // anim_y.push_back(y); - // anim_z.push_back(z); - - // // Plot the partial trajectory so far (in black dotted line) - // auto path_plot = plot3(anim_x, anim_y, anim_z); - // path_plot->line_width(1.5); - // path_plot->line_style("--"); - // path_plot->color("black"); - - // // Build rotation from quaternion - // Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - // Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // // Arm endpoints (front, right, back, left) - // std::vector arm_endpoints; - // arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // // Transform to world coords - // for (auto &pt : arm_endpoints) - // { - // pt = Eigen::Vector3d(x, y, z) + R * pt; - // } - - // // Front-back arm - // std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - // std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - // std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - // auto fb_arm = plot3(fx, fy, fz); - // fb_arm->line_width(2.0); - // fb_arm->color("blue"); - - // // Right-left arm - // std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - // std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - // std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - // auto rl_arm = plot3(rx, ry, rz); - // rl_arm->line_width(2.0); - // rl_arm->color("red"); - - // auto sphere_points = linspace(0, 2 * M_PI, 15); - // for (const auto &motor_pos : arm_endpoints) - // { - // std::vector circ_x, circ_y, circ_z; - // circ_x.reserve(sphere_points.size()); - // circ_y.reserve(sphere_points.size()); - // circ_z.reserve(sphere_points.size()); - // for (auto angle : sphere_points) - // { - // circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - // circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - // circ_z.push_back(motor_pos.z()); // keep the same z for a small ring - // } - // auto sphere_plot = plot3(circ_x, circ_y, circ_z); - // sphere_plot->line_style("solid"); - // sphere_plot->line_width(1.5); - // sphere_plot->color("cyan"); - // } - - // title(ax_anim, "Quadrotor Animation"); - // xlabel(ax_anim, "X [m]"); - // ylabel(ax_anim, "Y [m]"); - // zlabel(ax_anim, "Z [m]"); - // xlim(ax_anim, {-5, 5}); - // ylim(ax_anim, {-5, 5}); - // zlim(ax_anim, {0, 5}); - - // ax_anim->view(30, -30); - - // std::string frameFile = plotDirectory + "/quadrotor_anim_frame_" + std::to_string(i / frame_stride) + ".png"; - // f_anim->draw(); - // f_anim->save(frameFile); - // std::this_thread::sleep_for(std::chrono::milliseconds(80)); - // } - - // // ----------------------------- - // // Generate GIF from frames using ImageMagick - // // ----------------------------- - // std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_anim_frame_*.png " + plotDirectory + "/quadrotor_circle.gif"; - // std::system(gif_command.c_str()); - - // std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_anim_frame_*.png"; - // std::system(cleanup_command.c_str()); - return 0; -} - -// To create a gif from the saved frames, use ImageMagick (for example): -// convert -delay 5 ../results/tests/quadrotor_frame_*.png ../results/tests/quadrotor_circle.gif diff --git a/examples/cddp_quadrotor_figure_eight_horizontal.cpp b/examples/cddp_quadrotor_figure_eight_horizontal.cpp deleted file mode 100644 index 04c0771a..00000000 --- a/examples/cddp_quadrotor_figure_eight_horizontal.cpp +++ /dev/null @@ -1,556 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -// Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - return R; -} - -// Transform quadrotor frame points (motor positions) to world coordinates using quaternion -std::vector> transformQuadrotorFrame( - const Eigen::Vector3d &position, - const Eigen::Vector4d &quat, // [qw, qx, qy, qz] - double arm_length) -{ - // Motor positions in body frame - std::vector body_points = { - Eigen::Vector3d(arm_length, 0, 0), // Front - Eigen::Vector3d(0, arm_length, 0), // Right - Eigen::Vector3d(-arm_length, 0, 0), // Back - Eigen::Vector3d(0, -arm_length, 0) // Left - }; - - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat[0], quat[1], quat[2], quat[3]); - - // Prepare return container - std::vector> world_points(3, std::vector()); - for (const auto &pt : body_points) - { - Eigen::Vector3d wp = position + R * pt; - world_points[0].push_back(wp.x()); - world_points[1].push_back(wp.y()); - world_points[2].push_back(wp.z()); - } - return world_points; -} - -int main() -{ - // For quaternion-based quadrotor, state_dim = 13: - // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - int state_dim = 13; - int control_dim = 4; // [f1, f2, f3, f4] - int horizon = 400; - double timestep = 0.02; - - // Quadrotor parameters - double mass = 1.2; // 1 kg - double arm_length = 0.165; // 20 cm - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 7.782e-3; // Ixx - inertia_matrix(1, 1) = 7.782e-3; // Iyy - inertia_matrix(2, 2) = 1.439e-2; // Izz - - std::string integration_type = "rk4"; - - // Create the dynamical system - std::unique_ptr system = - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type); - - // For propagation, create a direct instance - cddp::Quadrotor quadrotor(timestep, mass, inertia_matrix, arm_length, integration_type); - - // Cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - // penalize [x, y, z, qw, qx, qy, qz] more (the orientation/quaternion part) - Q(0, 0) = 1.0; - Q(1, 1) = 1.0; - Q(2, 2) = 1.0; - Q(3, 3) = 1.0; - Q(4, 4) = 1.0; - Q(5, 5) = 1.0; - Q(6, 6) = 1.0; - - Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 1.0; - Qf(1, 1) = 1.0; - Qf(2, 2) = 1.0; - Qf(3, 3) = 1.0; - Qf(4, 4) = 1.0; - Qf(5, 5) = 1.0; - Qf(6, 6) = 1.0; - - // Figure-8 trajectory parameters - double figure8_scale = 3.0; // 3m - double constant_altitude = 2.0; // 2m - double total_time = horizon * timestep; - double omega = 2.0 * M_PI / total_time; // completes 1 cycle over the horizon - - std::vector figure8_reference_states; - figure8_reference_states.reserve(horizon + 1); - - for (int i = 0; i <= horizon; ++i) - { - double t = i * timestep; - double angle = omega * t; - - // Lemniscate of Gerono for (x, y) - // x = A cos(angle) - // y = A sin(angle)*cos(angle) - Eigen::VectorXd ref_state = Eigen::VectorXd::Zero(state_dim); - ref_state(0) = figure8_scale * std::cos(angle); - ref_state(1) = figure8_scale * std::sin(angle) * std::cos(angle); - ref_state(2) = constant_altitude; - - // Identity quaternion: [1, 0, 0, 0] - ref_state(3) = 1.0; - ref_state(4) = 0.0; - ref_state(5) = 0.0; - ref_state(6) = 0.0; - - figure8_reference_states.push_back(ref_state); - } - - // Hover at the starting point of the figure-8 - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state(0) = figure8_scale; // x - goal_state(2) = constant_altitude; - goal_state(3) = 1.0; // qw - - // Create the objective - auto objective = std::make_unique( - Q, R, Qf, goal_state, figure8_reference_states, timestep); - - // Start the same figure-8 starting point - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state(0) = figure8_scale; - initial_state(2) = constant_altitude; - initial_state(3) = 1.0; - - // Create CDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 10000; - options.tolerance = 1e-6; - options.verbose = true; - options.debug = false; - options.enable_parallel = true; - options.num_threads = 10; - options.regularization.initial_value = 1e-4; - options.ipddp.barrier.mu_initial = 1e-1; - options.use_ilqr = true; - options.msipddp.segment_length = horizon / 10; - options.msipddp.rollout_type = "nonlinear"; - - // Instantiate CDDP solver - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::move(system), - std::move(objective), - options); - - // Control constraints - double min_force = 0.0; - double max_force = 4.0; - Eigen::VectorXd control_upper_bound = max_force * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_lower_bound = min_force * Eigen::VectorXd::Ones(control_dim); - // cddp_solver.addConstraint("ControlConstraint", std::make_unique(control_lower_bound, control_upper_bound)); - - // Initial trajectory guess - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - double hover_thrust = mass * 9.81 / 4.0; - for (auto &u : U) - { - u = hover_thrust * Eigen::VectorXd::Ones(control_dim); - } - - // X[0] = initial_state; - // for (int i = 0; i < horizon; ++i) - // { - // X[i + 1] = quadrotor.getDiscreteDynamics(X[i], U[i]); - // } - X = figure8_reference_states; - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::cout << "Final state = " << X_sol.back().transpose() << std::endl; - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void)std::system("mkdir -p frames"); - - // Extract trajectory data - std::vector time_arr, x_arr, y_arr, z_arr; - std::vector phi_arr, theta_arr, psi_arr; - std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - time_arr.reserve(X_sol.size()); - x_arr.reserve(X_sol.size()); - y_arr.reserve(X_sol.size()); - z_arr.reserve(X_sol.size()); - phi_arr.reserve(X_sol.size()); - theta_arr.reserve(X_sol.size()); - psi_arr.reserve(X_sol.size()); - - qw_arr.reserve(X_sol.size()); - qx_arr.reserve(X_sol.size()); - qy_arr.reserve(X_sol.size()); - qz_arr.reserve(X_sol.size()); - q_norm_arr.reserve(X_sol.size()); - - for (size_t i = 0; i < X_sol.size(); ++i) - { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - - double qw = X_sol[i](3); - double qx = X_sol[i](4); - double qy = X_sol[i](5); - double qz = X_sol[i](6); - - // Euler angles - Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - phi_arr.push_back(euler(0)); - theta_arr.push_back(euler(1)); - psi_arr.push_back(euler(2)); - - // Quaternion data - qw_arr.push_back(qw); - qx_arr.push_back(qx); - qy_arr.push_back(qy); - qz_arr.push_back(qz); - q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - } - - // Control data - std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - std::vector f1_arr, f2_arr, f3_arr, f4_arr; - f1_arr.reserve(U_sol.size()); - f2_arr.reserve(U_sol.size()); - f3_arr.reserve(U_sol.size()); - f4_arr.reserve(U_sol.size()); - - for (const auto &u : U_sol) - { - f1_arr.push_back(u(0)); - f2_arr.push_back(u(1)); - f3_arr.push_back(u(2)); - f4_arr.push_back(u(3)); - } - - // Plotting - auto f1 = figure(); - f1->size(1200, 900); // Slightly bigger if you like - - // (1) Position - auto ax1_f1 = subplot(4, 1, 0); - auto plot_handle1 = plot(ax1_f1, time_arr, x_arr, "-r"); - plot_handle1->display_name("x"); - plot_handle1->line_width(2); - hold(ax1_f1, true); - auto plot_handle2 = plot(ax1_f1, time_arr, y_arr, "-g"); - plot_handle2->display_name("y"); - plot_handle2->line_width(2); - auto plot_handle3 = plot(ax1_f1, time_arr, z_arr, "-b"); - plot_handle3->display_name("z"); - plot_handle3->line_width(2); - - title(ax1_f1, "Position Trajectories"); - xlabel(ax1_f1, "Time [s]"); - ylabel(ax1_f1, "Position [m]"); - matplot::legend(ax1_f1); - grid(ax1_f1, true); - - // (2) Attitude (Euler angles) - auto ax2_f1 = subplot(4, 1, 1); - hold(ax2_f1, true); - auto plot_handle4 = plot(ax2_f1, time_arr, phi_arr, "-r"); - plot_handle4->display_name("roll"); - plot_handle4->line_width(2); - auto plot_handle5 = plot(ax2_f1, time_arr, theta_arr, "-g"); - plot_handle5->display_name("pitch"); - plot_handle5->line_width(2); - auto plot_handle6 = plot(ax2_f1, time_arr, psi_arr, "-b"); - plot_handle6->display_name("yaw"); - plot_handle6->line_width(2); - - title(ax2_f1, "Attitude Angles"); - xlabel(ax2_f1, "Time [s]"); - ylabel(ax2_f1, "Angle [rad]"); - matplot::legend(ax2_f1); - grid(ax2_f1, true); - - // (3) Motor forces - auto ax3_f1 = subplot(4, 1, 2); - auto plot_handle7 = plot(ax3_f1, time_arr2, f1_arr, "-r"); - plot_handle7->display_name("f1"); - plot_handle7->line_width(2); - hold(ax3_f1, true); - auto plot_handle8 = plot(ax3_f1, time_arr2, f2_arr, "-g"); - plot_handle8->display_name("f2"); - plot_handle8->line_width(2); - auto plot_handle9 = plot(ax3_f1, time_arr2, f3_arr, "-b"); - plot_handle9->display_name("f3"); - plot_handle9->line_width(2); - auto plot_handle10 = plot(ax3_f1, time_arr2, f4_arr, "-k"); - plot_handle10->display_name("f4"); - plot_handle10->line_width(2); - - title(ax3_f1, "Motor Forces"); - xlabel(ax3_f1, "Time [s]"); - ylabel(ax3_f1, "Force [N]"); - matplot::legend(ax3_f1); - grid(ax3_f1, true); - - // (4) Quaternion trajectories and norm - auto ax4_f1 = subplot(4, 1, 3); - hold(ax4_f1, true); - - auto qwh = plot(ax4_f1, time_arr, qw_arr, "-r"); - qwh->display_name("q_w"); - qwh->line_width(2); - - auto qxh = plot(ax4_f1, time_arr, qx_arr, "-g"); - qxh->display_name("q_x"); - qxh->line_width(2); - - auto qyh = plot(ax4_f1, time_arr, qy_arr, "-b"); - qyh->display_name("q_y"); - qyh->line_width(2); - - auto qzh = plot(ax4_f1, time_arr, qz_arr, "-m"); - qzh->display_name("q_z"); - qzh->line_width(2); - - // Norm in black - auto qnorm_handle = plot(ax4_f1, time_arr, q_norm_arr, "-k"); - qnorm_handle->display_name("|q|"); - - title(ax4_f1, "Quaternion Components and Norm"); - xlabel(ax4_f1, "Time [s]"); - ylabel(ax4_f1, "Value"); - matplot::legend(ax4_f1); - grid(ax4_f1, true); - - // Save figure 1 - f1->draw(); - f1->save(plotDirectory + "/quadrotor_figure_eight_horizontal_states.png"); - - // 3D Trajectory - auto f2 = figure(); - f2->size(800, 600); - auto ax2 = f2->current_axes(); - hold(ax2, true); - auto traj3d = plot3(ax2, x_arr, y_arr, z_arr); - traj3d->display_name("Trajectory"); - traj3d->line_style("-"); - traj3d->line_width(2); - traj3d->color("blue"); - // Project trajectory onto x-y plane at z=0 - auto proj_xy = plot3(ax2, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - proj_xy->display_name("X-Y Projection"); - proj_xy->line_style("--"); - proj_xy->line_width(1); - proj_xy->color("gray"); - xlabel(ax2, "X [m]"); - ylabel(ax2, "Y [m]"); - zlabel(ax2, "Z [m]"); - xlim(ax2, {-5, 5}); - ylim(ax2, {-2, 2}); - zlim(ax2, {0, 5}); - title(ax2, "3D Trajectory (Figure-8)"); - grid(ax2, true); - f2->draw(); - f2->save(plotDirectory + "/quadrotor_figure_eight_horizontal_3d.png"); - - // Animation of the quadrotor frame - auto f_anim = figure(); - f_anim->size(800, 600); - auto ax_anim = f_anim->current_axes(); - - // For collecting the trajectory as we go - std::vector anim_x, anim_y, anim_z; - anim_x.reserve(X_sol.size()); - anim_y.reserve(X_sol.size()); - anim_z.reserve(X_sol.size()); - - // Render every Nth frame to reduce #images - int frame_stride = 15; - double prop_radius = 0.03; // radius for small spheres at motor ends - - for (size_t i = 0; i < X_sol.size(); i += frame_stride) - { - ax_anim->clear(); - ax_anim->hold(true); - ax_anim->grid(true); - - // Current state - double x = X_sol[i](0); - double y = X_sol[i](1); - double z = X_sol[i](2); - - // Accumulate path - anim_x.push_back(x); - anim_y.push_back(y); - anim_z.push_back(z); - - // Plot the partial trajectory so far (in black dotted line) - auto path_plot = plot3(anim_x, anim_y, anim_z); - path_plot->line_width(1.5); - path_plot->line_style("--"); - path_plot->color("black"); - - // Build rotation from quaternion - Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // Arm endpoints (front, right, back, left) - std::vector arm_endpoints; - arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // Transform to world coords - for (auto &pt : arm_endpoints) - { - pt = Eigen::Vector3d(x, y, z) + R * pt; - } - - // Front-back arm - std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - auto fb_arm = plot3(fx, fy, fz); - fb_arm->line_width(2.0); - fb_arm->color("blue"); - - // Right-left arm - std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - auto rl_arm = plot3(rx, ry, rz); - rl_arm->line_width(2.0); - rl_arm->color("red"); - - auto sphere_points = linspace(0, 2 * M_PI, 15); - for (const auto &motor_pos : arm_endpoints) - { - std::vector circ_x, circ_y, circ_z; - circ_x.reserve(sphere_points.size()); - circ_y.reserve(sphere_points.size()); - circ_z.reserve(sphere_points.size()); - for (auto angle : sphere_points) - { - circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - circ_z.push_back(motor_pos.z()); // keep the same z for a small ring - } - auto sphere_plot = plot3(circ_x, circ_y, circ_z); - sphere_plot->line_style("solid"); - sphere_plot->line_width(1.5); - sphere_plot->color("cyan"); - } - - title(ax_anim, "Quadrotor Animation"); - xlabel(ax_anim, "X [m]"); - ylabel(ax_anim, "Y [m]"); - zlabel(ax_anim, "Z [m]"); - xlim(ax_anim, {-5, 5}); - ylim(ax_anim, {-5, 5}); - zlim(ax_anim, {0, 5}); - - ax_anim->view(30, -30); - - std::string frameFile = plotDirectory + "/quadrotor_anim_frame_" + std::to_string(i / frame_stride) + ".png"; - f_anim->draw(); - f_anim->save(frameFile); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_anim_frame_*.png " + plotDirectory + "/quadrotor_figure_eight_horizontal.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_anim_frame_*.png"; - std::system(cleanup_command.c_str()); - - return 0; -} diff --git a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp b/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp deleted file mode 100644 index f26ffee5..00000000 --- a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp +++ /dev/null @@ -1,625 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -// Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - return R; -} - -// Transform quadrotor frame points (motor positions) to world coordinates using quaternion -std::vector> transformQuadrotorFrame( - const Eigen::Vector3d &position, - const Eigen::Vector4d &quat, // [qw, qx, qy, qz] - double arm_length) -{ - // Motor positions in body frame - std::vector body_points = { - Eigen::Vector3d(arm_length, 0, 0), // Front - Eigen::Vector3d(0, arm_length, 0), // Right - Eigen::Vector3d(-arm_length, 0, 0), // Back - Eigen::Vector3d(0, -arm_length, 0) // Left - }; - - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat[0], quat[1], quat[2], quat[3]); - - // Prepare return container - std::vector> world_points(3, std::vector()); - for (const auto &pt : body_points) - { - Eigen::Vector3d wp = position + R * pt; - world_points[0].push_back(wp.x()); - world_points[1].push_back(wp.y()); - world_points[2].push_back(wp.z()); - } - return world_points; -} - -int main() -{ - // For quaternion-based quadrotor, state_dim = 13: - // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - int state_dim = 13; - int control_dim = 4; // [f1, f2, f3, f4] - int horizon = 400; - double timestep = 0.02; - - // Quadrotor parameters - double mass = 1.2; // 1 kg - double arm_length = 0.165; // 20 cm - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 7.782e-3; // Ixx - inertia_matrix(1, 1) = 7.782e-3; // Iyy - inertia_matrix(2, 2) = 1.439e-2; // Izz - - std::string integration_type = "rk4"; - - // Create the dynamical system - std::unique_ptr system = - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type); - - // For propagation, create a direct instance - cddp::Quadrotor quadrotor(timestep, mass, inertia_matrix, arm_length, integration_type); - - // Cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - // penalize [x, y, z, qw, qx, qy, qz] more (the orientation/quaternion part) - Q(0, 0) = 1.0; - Q(1, 1) = 1.0; - Q(2, 2) = 1.0; - // Q(3, 3) = 1.0; - // Q(4, 4) = 1.0; - // Q(5, 5) = 1.0; - // Q(6, 6) = 1.0; - - Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 1.0; - Qf(1, 1) = 1.0; - Qf(2, 2) = 1.0; - Qf(3, 3) = 1.0; - Qf(4, 4) = 1.0; - Qf(5, 5) = 1.0; - Qf(6, 6) = 1.0; - - // Figure-8 trajectory parameters - double figure8_scale = 3.0; // 3m - double constant_altitude = 2.0; // 2m - double total_time = horizon * timestep; - double omega = 2.0 * M_PI / total_time; // completes 1 cycle over the horizon - - std::vector figure8_reference_states; - figure8_reference_states.reserve(horizon + 1); - - for (int i = 0; i <= horizon; ++i) - { - double t = i * timestep; - double angle = omega * t; - - // Lemniscate of Gerono for (x, y) - // x = A cos(angle) - // y = A sin(angle)*cos(angle) - Eigen::VectorXd ref_state = Eigen::VectorXd::Zero(state_dim); - ref_state(0) = figure8_scale * std::cos(angle); - ref_state(1) = figure8_scale * std::sin(angle) * std::cos(angle); - ref_state(2) = constant_altitude; - - // Identity quaternion: [1, 0, 0, 0] - ref_state(3) = 1.0; - ref_state(4) = 0.0; - ref_state(5) = 0.0; - ref_state(6) = 0.0; - - figure8_reference_states.push_back(ref_state); - } - - // Hover at the starting point of the figure-8 - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state(0) = figure8_scale; // x - goal_state(2) = constant_altitude; - goal_state(3) = 1.0; // qw - - // Create the objective - auto objective = std::make_unique( - Q, R, Qf, goal_state, figure8_reference_states, timestep); - - // Start the same figure-8 starting point - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state(0) = figure8_scale; - initial_state(2) = constant_altitude; - initial_state(3) = 1.0; - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 100; - options.verbose = true; - options.debug = false; - options.tolerance = 1e-6; - options.acceptable_tolerance = 1e-7; - options.use_ilqr = true; - options.enable_parallel = false; - options.num_threads = 1; - - // Line search options - options.line_search.max_iterations = 10; - - // Regularization options - options.regularization.initial_value = 1e-3; - - // IPDDP-specific options - options.ipddp.barrier.mu_initial = 1e-1; - - // MSIPDDP-specific options - options.msipddp.barrier.mu_initial = 1e-1; - options.msipddp.segment_length = 2; - options.msipddp.rollout_type = "nonlinear"; - options.msipddp.use_controlled_rollout = false; - - // Instantiate CDDP solver - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::move(system), - std::move(objective), - options); - - // Control constraints - double min_force = 0.0; - double max_force = 4.0; - Eigen::VectorXd control_upper_bound = max_force * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_lower_bound = min_force * Eigen::VectorXd::Ones(control_dim); - cddp_solver.addPathConstraint("ControlConstraint", std::make_unique(control_lower_bound, control_upper_bound)); - - // Initial trajectory guess (use reference trajectory like benchmark) - double hover_thrust = mass * 9.81 / 4.0; - std::vector X_init = figure8_reference_states; // Use reference as initial guess - std::vector U_init(horizon, hover_thrust * Eigen::VectorXd::Ones(control_dim)); - cddp_solver.setInitialTrajectory(X_init, U_init); - - // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); - - options.max_iterations = 500; - options.warm_start = true; - - // Resolve problem with the ball constraint - cddp::CDDP solver_ball( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type), - std::make_unique( - Q, R, Qf, goal_state, figure8_reference_states, timestep), - options); - solver_ball.addPathConstraint("ControlConstraint", std::make_unique(control_lower_bound, control_upper_bound)); - - // Ball constraint - double ball_radius = 0.5; // 50 cm - Eigen::Vector3d ball_center(0.0, 0.0, constant_altitude); // Center of the ball - solver_ball.addPathConstraint("BallConstraint", std::make_unique(ball_radius, ball_center)); - - // Initial trajectory guess (extract from solution) - const auto& initial_X = solution.state_trajectory; - const auto& initial_U = solution.control_trajectory; - solver_ball.setInitialTrajectory(initial_X, initial_U); - - // Solve the problem (MSIPDDP, IPDDP) - cddp::CDDPSolution solution_ball = solver_ball.solve("MSIPDDP"); - - const auto& X_sol = solution_ball.state_trajectory; - const auto& U_sol = solution_ball.control_trajectory; - const auto& t_sol = solution_ball.time_points; - - std::cout << "Final state = " << X_sol.back().transpose() << std::endl; - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Create a directory for frame images. - (void)std::system("mkdir -p frames"); - - // Extract trajectory data - std::vector time_arr, x_arr, y_arr, z_arr; - std::vector phi_arr, theta_arr, psi_arr; - std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - time_arr.reserve(X_sol.size()); - x_arr.reserve(X_sol.size()); - y_arr.reserve(X_sol.size()); - z_arr.reserve(X_sol.size()); - phi_arr.reserve(X_sol.size()); - theta_arr.reserve(X_sol.size()); - psi_arr.reserve(X_sol.size()); - - qw_arr.reserve(X_sol.size()); - qx_arr.reserve(X_sol.size()); - qy_arr.reserve(X_sol.size()); - qz_arr.reserve(X_sol.size()); - q_norm_arr.reserve(X_sol.size()); - - for (size_t i = 0; i < X_sol.size(); ++i) - { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - - double qw = X_sol[i](3); - double qx = X_sol[i](4); - double qy = X_sol[i](5); - double qz = X_sol[i](6); - - // Euler angles - Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - phi_arr.push_back(euler(0)); - theta_arr.push_back(euler(1)); - psi_arr.push_back(euler(2)); - - // Quaternion data - qw_arr.push_back(qw); - qx_arr.push_back(qx); - qy_arr.push_back(qy); - qz_arr.push_back(qz); - q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - } - - // Control data - std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - std::vector f1_arr, f2_arr, f3_arr, f4_arr; - f1_arr.reserve(U_sol.size()); - f2_arr.reserve(U_sol.size()); - f3_arr.reserve(U_sol.size()); - f4_arr.reserve(U_sol.size()); - - for (const auto &u : U_sol) - { - f1_arr.push_back(u(0)); - f2_arr.push_back(u(1)); - f3_arr.push_back(u(2)); - f4_arr.push_back(u(3)); - } - - // Plotting - auto f1 = figure(); - f1->size(1200, 900); // Slightly bigger if you like - - // (1) Position - auto ax1_f1 = subplot(4, 1, 0); - auto plot_handle1 = plot(ax1_f1, time_arr, x_arr, "-r"); - plot_handle1->display_name("x"); - plot_handle1->line_width(2); - hold(ax1_f1, true); - auto plot_handle2 = plot(ax1_f1, time_arr, y_arr, "-g"); - plot_handle2->display_name("y"); - plot_handle2->line_width(2); - auto plot_handle3 = plot(ax1_f1, time_arr, z_arr, "-b"); - plot_handle3->display_name("z"); - plot_handle3->line_width(2); - - title(ax1_f1, "Position Trajectories"); - xlabel(ax1_f1, "Time [s]"); - ylabel(ax1_f1, "Position [m]"); - matplot::legend(ax1_f1); - grid(ax1_f1, true); - - // (2) Attitude (Euler angles) - auto ax2_f1 = subplot(4, 1, 1); - hold(ax2_f1, true); - auto plot_handle4 = plot(ax2_f1, time_arr, phi_arr, "-r"); - plot_handle4->display_name("roll"); - plot_handle4->line_width(2); - auto plot_handle5 = plot(ax2_f1, time_arr, theta_arr, "-g"); - plot_handle5->display_name("pitch"); - plot_handle5->line_width(2); - auto plot_handle6 = plot(ax2_f1, time_arr, psi_arr, "-b"); - plot_handle6->display_name("yaw"); - plot_handle6->line_width(2); - - title(ax2_f1, "Attitude Angles"); - xlabel(ax2_f1, "Time [s]"); - ylabel(ax2_f1, "Angle [rad]"); - matplot::legend(ax2_f1); - grid(ax2_f1, true); - - // (3) Motor forces - auto ax3_f1 = subplot(4, 1, 2); - auto plot_handle7 = plot(ax3_f1, time_arr2, f1_arr, "-r"); - plot_handle7->display_name("f1"); - plot_handle7->line_width(2); - hold(ax3_f1, true); - auto plot_handle8 = plot(ax3_f1, time_arr2, f2_arr, "-g"); - plot_handle8->display_name("f2"); - plot_handle8->line_width(2); - auto plot_handle9 = plot(ax3_f1, time_arr2, f3_arr, "-b"); - plot_handle9->display_name("f3"); - plot_handle9->line_width(2); - auto plot_handle10 = plot(ax3_f1, time_arr2, f4_arr, "-k"); - plot_handle10->display_name("f4"); - plot_handle10->line_width(2); - - title(ax3_f1, "Motor Forces"); - xlabel(ax3_f1, "Time [s]"); - ylabel(ax3_f1, "Force [N]"); - matplot::legend(ax3_f1); - grid(ax3_f1, true); - - // (4) Quaternion trajectories and norm - auto ax4_f1 = subplot(4, 1, 3); - hold(ax4_f1, true); - - auto qwh = plot(ax4_f1, time_arr, qw_arr, "-r"); - qwh->display_name("q_w"); - qwh->line_width(2); - - auto qxh = plot(ax4_f1, time_arr, qx_arr, "-g"); - qxh->display_name("q_x"); - qxh->line_width(2); - - auto qyh = plot(ax4_f1, time_arr, qy_arr, "-b"); - qyh->display_name("q_y"); - qyh->line_width(2); - - auto qzh = plot(ax4_f1, time_arr, qz_arr, "-m"); - qzh->display_name("q_z"); - qzh->line_width(2); - - // Norm in black - auto qnorm_handle = plot(ax4_f1, time_arr, q_norm_arr, "-k"); - qnorm_handle->display_name("|q|"); - - title(ax4_f1, "Quaternion Components and Norm"); - xlabel(ax4_f1, "Time [s]"); - ylabel(ax4_f1, "Value"); - matplot::legend(ax4_f1); - grid(ax4_f1, true); - - // Save figure 1 - f1->draw(); - f1->save(plotDirectory + "/quadrotor_figure_eight_horizontal_safe_states.png"); - - // 3D Trajectory - auto f2 = figure(); - f2->size(800, 600); - auto ax2 = f2->current_axes(); - hold(ax2, true); - auto traj3d = plot3(ax2, x_arr, y_arr, z_arr); - traj3d->display_name("Trajectory"); - traj3d->line_style("-"); - traj3d->line_width(2); - traj3d->color("blue"); - - // Project trajectory onto x-y plane at z=0 - auto proj_xy = plot3(ax2, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - proj_xy->display_name("X-Y Projection"); - proj_xy->line_style("--"); - proj_xy->line_width(1); - proj_xy->color("gray"); - - // Plot the ball constraint - int n_sphere = 30; - auto phi = linspace(0, M_PI, n_sphere); - auto theta = linspace(0, 2 * M_PI, n_sphere); - - std::vector> sx(n_sphere, std::vector(n_sphere)); - std::vector> sy(n_sphere, std::vector(n_sphere)); - std::vector> sz(n_sphere, std::vector(n_sphere)); - - for (int i = 0; i < n_sphere; i++) - { - for (int j = 0; j < n_sphere; j++) - { - sx[i][j] = ball_center(0) + ball_radius * std::sin(phi[i]) * std::cos(theta[j]); - sy[i][j] = ball_center(1) + ball_radius * std::sin(phi[i]) * std::sin(theta[j]); - sz[i][j] = ball_center(2) + ball_radius * std::cos(phi[i]); - } - } - auto sphere_surf = surf(ax2, sx, sy, sz); - sphere_surf->edge_color("red"); - - // Plot projection of the sphere onto the x-y plane - int n_points = 100; - std::vector circle_x(n_points), circle_y(n_points), circle_z(n_points); - for (int i = 0; i < n_points; i++) - { - double angle = 2.0 * M_PI * i / n_points; - circle_x[i] = ball_center(0) + ball_radius * std::cos(angle); - circle_y[i] = ball_center(1) + ball_radius * std::sin(angle); - circle_z[i] = 0.0; // Projection onto x-y plane - } - // Draw the circle perimeter - auto circle_plot = plot3(ax2, circle_x, circle_y, circle_z); - circle_plot->line_width(2).line_style("--").display_name("Ball x-y projection"); - circle_plot->color("black"); // For contrast - - - xlabel(ax2, "X [m]"); - ylabel(ax2, "Y [m]"); - zlabel(ax2, "Z [m]"); - xlim(ax2, {-5, 5}); - ylim(ax2, {-2, 2}); - zlim(ax2, {0, 5}); - title(ax2, "3D Trajectory (Figure-8)"); - grid(ax2, true); - f2->draw(); - f2->save(plotDirectory + "/quadrotor_figure_eight_horizontal_safe_3d.png"); - f2->show(); - - // // Animation of the quadrotor frame - // auto f_anim = figure(); - // f_anim->size(800, 600); - // auto ax_anim = f_anim->current_axes(); - - // // For collecting the trajectory as we go - // std::vector anim_x, anim_y, anim_z; - // anim_x.reserve(X_sol.size()); - // anim_y.reserve(X_sol.size()); - // anim_z.reserve(X_sol.size()); - - // // Render every Nth frame to reduce #images - // int frame_stride = 15; - // double prop_radius = 0.03; // radius for small spheres at motor ends - - // for (size_t i = 0; i < X_sol.size(); i += frame_stride) - // { - // ax_anim->clear(); - // ax_anim->hold(true); - // ax_anim->grid(true); - - // // Current state - // double x = X_sol[i](0); - // double y = X_sol[i](1); - // double z = X_sol[i](2); - - // // Accumulate path - // anim_x.push_back(x); - // anim_y.push_back(y); - // anim_z.push_back(z); - - // // Plot the partial trajectory so far (in black dotted line) - // auto path_plot = plot3(anim_x, anim_y, anim_z); - // path_plot->line_width(1.5); - // path_plot->line_style("--"); - // path_plot->color("black"); - - // // Build rotation from quaternion - // Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - // Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // // Arm endpoints (front, right, back, left) - // std::vector arm_endpoints; - // arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - // arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // // Transform to world coords - // for (auto &pt : arm_endpoints) - // { - // pt = Eigen::Vector3d(x, y, z) + R * pt; - // } - - // // Front-back arm - // std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - // std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - // std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - // auto fb_arm = plot3(fx, fy, fz); - // fb_arm->line_width(2.0); - // fb_arm->color("blue"); - - // // Right-left arm - // std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - // std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - // std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - // auto rl_arm = plot3(rx, ry, rz); - // rl_arm->line_width(2.0); - // rl_arm->color("red"); - - // auto sphere_points = linspace(0, 2 * M_PI, 15); - // for (const auto &motor_pos : arm_endpoints) - // { - // std::vector circ_x, circ_y, circ_z; - // circ_x.reserve(sphere_points.size()); - // circ_y.reserve(sphere_points.size()); - // circ_z.reserve(sphere_points.size()); - // for (auto angle : sphere_points) - // { - // circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - // circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - // circ_z.push_back(motor_pos.z()); // keep the same z for a small ring - // } - // auto sphere_plot = plot3(circ_x, circ_y, circ_z); - // sphere_plot->line_style("solid"); - // sphere_plot->line_width(1.5); - // sphere_plot->color("cyan"); - // } - - // title(ax_anim, "Quadrotor Animation"); - // xlabel(ax_anim, "X [m]"); - // ylabel(ax_anim, "Y [m]"); - // zlabel(ax_anim, "Z [m]"); - // xlim(ax_anim, {-5, 5}); - // ylim(ax_anim, {-5, 5}); - // zlim(ax_anim, {0, 5}); - - // ax_anim->view(30, -30); - - // std::string frameFile = plotDirectory + "/quadrotor_anim_frame_" + std::to_string(i / frame_stride) + ".png"; - // f_anim->draw(); - // f_anim->save(frameFile); - // std::this_thread::sleep_for(std::chrono::milliseconds(80)); - // } - - // // ----------------------------- - // // Generate GIF from frames using ImageMagick - // // ----------------------------- - // std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_anim_frame_*.png " + plotDirectory + "/quadrotor_figure_eight_horizontal_safe.gif"; - // std::system(gif_command.c_str()); - - // std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_anim_frame_*.png"; - // std::system(cleanup_command.c_str()); - - return 0; -} diff --git a/examples/cddp_quadrotor_figure_eight_vertical.cpp b/examples/cddp_quadrotor_figure_eight_vertical.cpp deleted file mode 100644 index ba288c05..00000000 --- a/examples/cddp_quadrotor_figure_eight_vertical.cpp +++ /dev/null @@ -1,555 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; -// Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - return R; -} - -// Transform quadrotor frame points (motor positions) to world coordinates using quaternion -std::vector> transformQuadrotorFrame( - const Eigen::Vector3d &position, - const Eigen::Vector4d &quat, // [qw, qx, qy, qz] - double arm_length) -{ - // Motor positions in body frame - std::vector body_points = { - Eigen::Vector3d(arm_length, 0, 0), // Front - Eigen::Vector3d(0, arm_length, 0), // Right - Eigen::Vector3d(-arm_length, 0, 0), // Back - Eigen::Vector3d(0, -arm_length, 0) // Left - }; - - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat[0], quat[1], quat[2], quat[3]); - - // Prepare return container - std::vector> world_points(3, std::vector()); - for (const auto &pt : body_points) - { - Eigen::Vector3d wp = position + R * pt; - world_points[0].push_back(wp.x()); - world_points[1].push_back(wp.y()); - world_points[2].push_back(wp.z()); - } - return world_points; -} - -int main() -{ - // For quaternion-based quadrotor, state_dim = 13: - // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - int state_dim = 13; - int control_dim = 4; // [f1, f2, f3, f4] - int horizon = 400; - double timestep = 0.02; - - // Quadrotor parameters - double mass = 1.2; // 1 kg - double arm_length = 0.165; // 20 cm - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 7.782e-3; // Ixx - inertia_matrix(1, 1) = 7.782e-3; // Iyy - inertia_matrix(2, 2) = 1.439e-2; // Izz - - std::string integration_type = "rk4"; - - // Create the dynamical system - std::unique_ptr system = - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type); - - // For propagation, create a direct instance - cddp::Quadrotor quadrotor(timestep, mass, inertia_matrix, arm_length, integration_type); - - // Cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - // penalize [x, y, z, qw, qx, qy, qz] more (the orientation/quaternion part) - Q(0, 0) = 1.0; - Q(1, 1) = 1.0; - Q(2, 2) = 1.0; - Q(3, 3) = 1.0; - Q(4, 4) = 1.0; - Q(5, 5) = 1.0; - Q(6, 6) = 1.0; - - Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 1.0; - Qf(1, 1) = 1.0; - Qf(2, 2) = 1.0; - Qf(3, 3) = 1.0; - Qf(4, 4) = 1.0; - Qf(5, 5) = 1.0; - Qf(6, 6) = 1.0; - - // Figure-8 trajectory parameters - double figure8_scale = 3.0; - double constant_altitude = 2.0; - double constant_y = 2.0; - double total_time = horizon * timestep; - double omega = 2.0 * M_PI / total_time; // completes 1 cycle over the horizon - - std::vector figure8_reference_states; - figure8_reference_states.reserve(horizon + 1); - - - for (int i = 0; i <= horizon; ++i) - { - double t = i * timestep; - double angle = omega * t; - - // Create a reference state of dimension state_dim (13) - Eigen::VectorXd ref_state = Eigen::VectorXd::Zero(state_dim); - - // Position (x, y, z) using a vertical lemniscate of Gerono in the x–z plane: - // x = figure8_scale * cos(angle) - // y = constant_y (fixed) - // z = constant_altitude + figure8_scale * sin(angle) * cos(angle) - ref_state(0) = figure8_scale * std::cos(angle); - ref_state(1) = constant_y; - ref_state(2) = constant_altitude + figure8_scale * std::sin(angle) * std::cos(angle); - - // Orientation: set to identity quaternion [1, 0, 0, 0] - ref_state(3) = 1.0; // qw - ref_state(4) = 0.0; // qx - ref_state(5) = 0.0; // qy - ref_state(6) = 0.0; // qz - - figure8_reference_states.push_back(ref_state); - } - - // Goal state: hover at the starting point of the figure-8 (x = figure8_scale, y = constant_y, z = constant_altitude) - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state(0) = figure8_scale; - goal_state(1) = constant_y; - goal_state(2) = constant_altitude; - goal_state(3) = 1.0; // Identity quaternion: qw = 1 - - auto objective = std::make_unique( - Q, R, Qf, goal_state, figure8_reference_states, timestep); - - // Initial state (at the start of the figure-8) - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state(0) = figure8_scale; - initial_state(1) = constant_y; - initial_state(2) = constant_altitude; - initial_state(3) = 1.0; // Identity quaternion: qw = 1 - - // Solver options - cddp::CDDPOptions options; - options.max_iterations = 10000; - options.verbose = true; - options.debug = false; - options.enable_parallel = true; - options.num_threads = 10; - options.tolerance = 1e-3; - options.regularization.initial_value = 1e-4; - options.ipddp.barrier.mu_initial = 1e-3; - - // Create the CDDP solver - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::move(system), - std::move(objective), - options); - - // Control constraints (motor thrust limits) - double min_force = 0.0; // Motors can only produce upward thrust - double max_force = 4.0; // Maximum thrust per motor - Eigen::VectorXd control_upper_bound = max_force * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_lower_bound = min_force * Eigen::VectorXd::Ones(control_dim); - cddp_solver.addPathConstraint("ControlConstraint", std::make_unique(-control_upper_bound, control_upper_bound)); - - // Initial trajectory: allocate state and control trajectories - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Initialize with hovering thrust (each motor provides mg/4) - double hover_thrust = mass * 9.81 / 4.0; - for (auto &u : U) - { - u = hover_thrust * Eigen::VectorXd::Ones(control_dim); - } - // Propagate initial trajectory using discrete dynamics - for (size_t i = 0; i < static_cast(horizon); ++i) - { - X[i + 1] = quadrotor.getDiscreteDynamics(X[i], U[i], i * timestep); - } - cddp_solver.setInitialTrajectory(X, U); - - // Solve the optimal control problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::cout << "Final state: " << X_sol.back().transpose() << std::endl; - - // Create plot directory if it doesn't exist - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - // Create a directory for frame images. - (void)std::system("mkdir -p frames"); - - // Extract trajectory data - std::vector time_arr, x_arr, y_arr, z_arr; - std::vector phi_arr, theta_arr, psi_arr; - std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - time_arr.reserve(X_sol.size()); - x_arr.reserve(X_sol.size()); - y_arr.reserve(X_sol.size()); - z_arr.reserve(X_sol.size()); - phi_arr.reserve(X_sol.size()); - theta_arr.reserve(X_sol.size()); - psi_arr.reserve(X_sol.size()); - - qw_arr.reserve(X_sol.size()); - qx_arr.reserve(X_sol.size()); - qy_arr.reserve(X_sol.size()); - qz_arr.reserve(X_sol.size()); - q_norm_arr.reserve(X_sol.size()); - - for (size_t i = 0; i < X_sol.size(); ++i) - { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - - double qw = X_sol[i](3); - double qx = X_sol[i](4); - double qy = X_sol[i](5); - double qz = X_sol[i](6); - - // Euler angles - Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - phi_arr.push_back(euler(0)); - theta_arr.push_back(euler(1)); - psi_arr.push_back(euler(2)); - - // Quaternion data - qw_arr.push_back(qw); - qx_arr.push_back(qx); - qy_arr.push_back(qy); - qz_arr.push_back(qz); - q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - } - - // Control data - std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - std::vector f1_arr, f2_arr, f3_arr, f4_arr; - f1_arr.reserve(U_sol.size()); - f2_arr.reserve(U_sol.size()); - f3_arr.reserve(U_sol.size()); - f4_arr.reserve(U_sol.size()); - - for (const auto &u : U_sol) - { - f1_arr.push_back(u(0)); - f2_arr.push_back(u(1)); - f3_arr.push_back(u(2)); - f4_arr.push_back(u(3)); - } - - // Plotting - auto f1 = figure(); - f1->size(1200, 900); // Slightly bigger if you like - - // (1) Position - auto ax1_f1 = subplot(4, 1, 0); - auto plot_handle1 = plot(ax1_f1, time_arr, x_arr, "-r"); - plot_handle1->display_name("x"); - plot_handle1->line_width(2); - hold(ax1_f1, true); - auto plot_handle2 = plot(ax1_f1, time_arr, y_arr, "-g"); - plot_handle2->display_name("y"); - plot_handle2->line_width(2); - auto plot_handle3 = plot(ax1_f1, time_arr, z_arr, "-b"); - plot_handle3->display_name("z"); - plot_handle3->line_width(2); - - title(ax1_f1, "Position Trajectories"); - xlabel(ax1_f1, "Time [s]"); - ylabel(ax1_f1, "Position [m]"); - matplot::legend(ax1_f1); - grid(ax1_f1, true); - - // (2) Attitude (Euler angles) - auto ax2_f1 = subplot(4, 1, 1); - hold(ax2_f1, true); - auto plot_handle4 = plot(ax2_f1, time_arr, phi_arr, "-r"); - plot_handle4->display_name("roll"); - plot_handle4->line_width(2); - auto plot_handle5 = plot(ax2_f1, time_arr, theta_arr, "-g"); - plot_handle5->display_name("pitch"); - plot_handle5->line_width(2); - auto plot_handle6 = plot(ax2_f1, time_arr, psi_arr, "-b"); - plot_handle6->display_name("yaw"); - plot_handle6->line_width(2); - - title(ax2_f1, "Attitude Angles"); - xlabel(ax2_f1, "Time [s]"); - ylabel(ax2_f1, "Angle [rad]"); - matplot::legend(ax2_f1); - grid(ax2_f1, true); - - // (3) Motor forces - auto ax3_f1 = subplot(4, 1, 2); - auto plot_handle7 = plot(ax3_f1, time_arr2, f1_arr, "-r"); - plot_handle7->display_name("f1"); - plot_handle7->line_width(2); - hold(ax3_f1, true); - auto plot_handle8 = plot(ax3_f1, time_arr2, f2_arr, "-g"); - plot_handle8->display_name("f2"); - plot_handle8->line_width(2); - auto plot_handle9 = plot(ax3_f1, time_arr2, f3_arr, "-b"); - plot_handle9->display_name("f3"); - plot_handle9->line_width(2); - auto plot_handle10 = plot(ax3_f1, time_arr2, f4_arr, "-k"); - plot_handle10->display_name("f4"); - plot_handle10->line_width(2); - - title(ax3_f1, "Motor Forces"); - xlabel(ax3_f1, "Time [s]"); - ylabel(ax3_f1, "Force [N]"); - matplot::legend(ax3_f1); - grid(ax3_f1, true); - - // (4) Quaternion trajectories and norm - auto ax4_f1 = subplot(4, 1, 3); - hold(ax4_f1, true); - - auto qwh = plot(ax4_f1, time_arr, qw_arr, "-r"); - qwh->display_name("q_w"); - qwh->line_width(2); - - auto qxh = plot(ax4_f1, time_arr, qx_arr, "-g"); - qxh->display_name("q_x"); - qxh->line_width(2); - - auto qyh = plot(ax4_f1, time_arr, qy_arr, "-b"); - qyh->display_name("q_y"); - qyh->line_width(2); - - auto qzh = plot(ax4_f1, time_arr, qz_arr, "-m"); - qzh->display_name("q_z"); - qzh->line_width(2); - - // Norm in black - auto qnorm_handle = plot(ax4_f1, time_arr, q_norm_arr, "-k"); - qnorm_handle->display_name("|q|"); - - title(ax4_f1, "Quaternion Components and Norm"); - xlabel(ax4_f1, "Time [s]"); - ylabel(ax4_f1, "Value"); - matplot::legend(ax4_f1); - grid(ax4_f1, true); - - // Save figure 1 - f1->draw(); - f1->save(plotDirectory + "/quadrotor_figure_eight_vertical_states.png"); - - // 3D Trajectory - auto f2 = figure(); - f2->size(800, 600); - auto ax2 = f2->current_axes(); - hold(ax2, true); - auto traj3d = plot3(ax2, x_arr, y_arr, z_arr); - traj3d->display_name("Trajectory"); - traj3d->line_style("-"); - traj3d->line_width(2); - traj3d->color("blue"); - // Project trajectory onto x-y plane at z=0 - auto proj_xy = plot3(ax2, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - proj_xy->display_name("X-Y Projection"); - proj_xy->line_style("--"); - proj_xy->line_width(1); - proj_xy->color("gray"); - xlabel(ax2, "X [m]"); - ylabel(ax2, "Y [m]"); - zlabel(ax2, "Z [m]"); - xlim(ax2, {-5, 5}); - ylim(ax2, {0, 5}); - zlim(ax2, {-5, 5}); - title(ax2, "3D Trajectory (Figure-8)"); - grid(ax2, true); - f2->draw(); - f2->save(plotDirectory + "/quadrotor_figure_eight_vertical_3d.png"); - - // Animation of the quadrotor frame - auto f_anim = figure(); - f_anim->size(800, 600); - auto ax_anim = f_anim->current_axes(); - - // For collecting the trajectory as we go - std::vector anim_x, anim_y, anim_z; - anim_x.reserve(X_sol.size()); - anim_y.reserve(X_sol.size()); - anim_z.reserve(X_sol.size()); - - // Render every Nth frame to reduce #images - int frame_stride = 15; - double prop_radius = 0.03; // radius for small spheres at motor ends - - for (size_t i = 0; i < X_sol.size(); i += frame_stride) - { - ax_anim->clear(); - ax_anim->hold(true); - ax_anim->grid(true); - - // Current state - double x = X_sol[i](0); - double y = X_sol[i](1); - double z = X_sol[i](2); - - // Accumulate path - anim_x.push_back(x); - anim_y.push_back(y); - anim_z.push_back(z); - - // Plot the partial trajectory so far (in black dotted line) - auto path_plot = plot3(anim_x, anim_y, anim_z); - path_plot->line_width(1.5); - path_plot->line_style("--"); - path_plot->color("black"); - - // Build rotation from quaternion - Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // Arm endpoints (front, right, back, left) - std::vector arm_endpoints; - arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // Transform to world coords - for (auto &pt : arm_endpoints) - { - pt = Eigen::Vector3d(x, y, z) + R * pt; - } - - // Front-back arm - std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - auto fb_arm = plot3(fx, fy, fz); - fb_arm->line_width(2.0); - fb_arm->color("blue"); - - // Right-left arm - std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - auto rl_arm = plot3(rx, ry, rz); - rl_arm->line_width(2.0); - rl_arm->color("red"); - - auto sphere_points = linspace(0, 2 * M_PI, 15); - for (const auto &motor_pos : arm_endpoints) - { - std::vector circ_x, circ_y, circ_z; - circ_x.reserve(sphere_points.size()); - circ_y.reserve(sphere_points.size()); - circ_z.reserve(sphere_points.size()); - for (auto angle : sphere_points) - { - circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - circ_z.push_back(motor_pos.z()); // keep the same z for a small ring - } - auto sphere_plot = plot3(circ_x, circ_y, circ_z); - sphere_plot->line_style("solid"); - sphere_plot->line_width(1.5); - sphere_plot->color("cyan"); - } - - title(ax_anim, "Quadrotor Animation"); - xlabel(ax_anim, "X [m]"); - ylabel(ax_anim, "Y [m]"); - zlabel(ax_anim, "Z [m]"); - xlim(ax_anim, {-5, 5}); - ylim(ax_anim, {0, 5}); - - ax_anim->view(30, -30); - - std::string frameFile = plotDirectory + "/quadrotor_anim_frame_" + std::to_string(i / frame_stride) + ".png"; - f_anim->draw(); - f_anim->save(frameFile); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_anim_frame_*.png " + plotDirectory + "/quadrotor_figure_eight_vertical.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_anim_frame_*.png"; - std::system(cleanup_command.c_str()); - - return 0; -} diff --git a/examples/cddp_quadrotor_point.cpp b/examples/cddp_quadrotor_point.cpp index 42fb07dc..5346d977 100644 --- a/examples/cddp_quadrotor_point.cpp +++ b/examples/cddp_quadrotor_point.cpp @@ -13,507 +13,99 @@ See the License for the specific language governing permissions and limitations under the License. */ + #include #include -#include -#include -#include #include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -// Helper function: Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Helper function: Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - return R; -} - -// Helper: Transform quadrotor frame points (motor positions) to world coordinates using quaternion. -std::vector> transformQuadrotorFrame( - const Eigen::Vector3d &position, - const Eigen::Vector4d &quat, // [qw, qx, qy, qz] - double arm_length) -{ - - // Define quadrotor motor positions in the body frame - std::vector body_points = { - Eigen::Vector3d(arm_length, 0, 0), // Front motor - Eigen::Vector3d(0, arm_length, 0), // Right motor - Eigen::Vector3d(-arm_length, 0, 0), // Back motor - Eigen::Vector3d(0, -arm_length, 0) // Left motor - }; - - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat[0], quat[1], quat[2], quat[3]); - - std::vector> world_points(3, std::vector()); - for (const auto &pt : body_points) - { - Eigen::Vector3d wp = position + R * pt; - world_points[0].push_back(wp.x()); - world_points[1].push_back(wp.y()); - world_points[2].push_back(wp.z()); - } - return world_points; -} - -int main() -{ - // For quaternion-based quadrotor, state_dim = 13: - // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - int state_dim = 13; - int control_dim = 4; // [f1, f2, f3, f4] - int horizon = 200; // Longer horizon for 3D maneuvers - double timestep = 0.02; +int main() { + constexpr int state_dim = 13; + constexpr int control_dim = 4; + constexpr int horizon = 120; + constexpr double timestep = 0.02; - // Quadrotor parameters - double mass = 1.0; // 1kg quadrotor - double arm_length = 0.2; // 20cm arm length - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 0.01; // Ixx - inertia_matrix(1, 1) = 0.01; // Iyy - inertia_matrix(2, 2) = 0.02; // Izz + const double mass = 1.0; + const double arm_length = 0.2; + Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero(); + inertia(0, 0) = 0.01; + inertia(1, 1) = 0.01; + inertia(2, 2) = 0.02; - std::string integration_type = "rk4"; - - // Create the dynamical system (quadrotor) as a unique_ptr - std::unique_ptr system = - std::make_unique(timestep, mass, inertia_matrix, arm_length, integration_type); + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + initial_state(3) = 1.0; - // For propagation (e.g., initial trajectory), also create an instance (if needed) - cddp::Quadrotor quadrotor(timestep, mass, inertia_matrix, arm_length, integration_type); + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state(0) = 3.0; + goal_state(2) = 2.0; + goal_state(3) = 1.0; - // Cost matrices (dimensions updated for state_dim = 13) Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); Q(4, 4) = 0.1; Q(5, 5) = 0.1; Q(6, 6) = 0.1; - // Control cost matrix (penalize aggressive control inputs) Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); - // Terminal cost matrix (important for stability) Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 500.0; // x position - Qf(1, 1) = 500.0; // y position - Qf(2, 2) = 500.0; // z position - // For orientation (quaternion) we penalize deviation from identity quaternion - Qf(3, 3) = 1.0; // qw - Qf(4, 4) = 1.0; // qx - Qf(5, 5) = 1.0; // qy - Qf(6, 6) = 1.0; // qz - Qf(7, 7) = 10.0; // x velocity - Qf(8, 8) = 10.0; // y velocity - Qf(9, 9) = 10.0; // z velocity - // Qf(10, 10) = 0.1; // roll rate - // Qf(11, 11) = 0.1; // pitch rate - // Qf(12, 12) = 0.1; // yaw rate - - // Goal state: hover at position (3,0,2) with identity quaternion and zero velocities. - Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); - goal_state(0) = 3.0; // x - goal_state(2) = 2.0; // z - // Set identity quaternion [1, 0, 0, 0] - goal_state(3) = 1.0; - goal_state(4) = 0.0; - goal_state(5) = 0.0; - goal_state(6) = 0.0; + Qf(0, 0) = 500.0; + Qf(1, 1) = 500.0; + Qf(2, 2) = 500.0; + Qf(3, 3) = 1.0; + Qf(4, 4) = 1.0; + Qf(5, 5) = 1.0; + Qf(6, 6) = 1.0; + Qf(7, 7) = 10.0; + Qf(8, 8) = 10.0; + Qf(9, 9) = 10.0; - // No intermediate reference states (optional) - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state (at origin with identity quaternion) - Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); - initial_state(3) = 1.0; // Identity quaternion: qw = 1 - - // Solver options cddp::CDDPOptions options; - options.max_iterations = 2000; + options.max_iterations = 120; options.line_search.max_iterations = 15; options.regularization.initial_value = 1e-4; - // Create the CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Control constraints (motor thrust limits) - double min_force = 0.0; // Motors can only produce thrust upward - double max_force = 5.0; // Maximum thrust per motor - Eigen::VectorXd control_lower_bound = min_force * Eigen::VectorXd::Ones(control_dim); - Eigen::VectorXd control_upper_bound = max_force * Eigen::VectorXd::Ones(control_dim); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // Initial trajectory: allocate state and control trajectories + cddp::CDDP solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique( + timestep, mass, inertia, arm_length, "rk4"), + std::make_unique( + Q, R, Qf, goal_state, std::vector{}, timestep), + options); + + const Eigen::VectorXd lower = + Eigen::VectorXd::Zero(control_dim); + const Eigen::VectorXd upper = + 5.0 * Eigen::VectorXd::Ones(control_dim); + solver.addPathConstraint("ControlConstraint", + std::make_unique(lower, upper)); + + cddp::Quadrotor propagation_model(timestep, mass, inertia, arm_length, "rk4"); std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - - // Initialize with hovering thrust (each motor provides mg/4) - double hover_thrust = mass * 9.81 / 4.0; - for (auto &u : U) - { + X[0] = initial_state; + const double hover_thrust = mass * 9.81 / 4.0; + for (auto &u : U) { u = hover_thrust * Eigen::VectorXd::Ones(control_dim); } - // Propagate initial trajectory using discrete dynamics - for (size_t i = 0; i < static_cast(horizon); ++i) - { - X[i + 1] = quadrotor.getDiscreteDynamics(X[i], U[i], i * timestep); - } - cddp_solver.setInitialTrajectory(X, U); - - // Solve the optimal control problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - const auto& X_sol = solution.state_trajectory; - const auto& U_sol = solution.control_trajectory; - const auto& t_sol = solution.time_points; - - std::cout << "Final state: " << X_sol.back().transpose() << std::endl; - - // Create plot directory if it doesn't exist - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - // Create a directory for frame images. - (void)std::system("mkdir -p frames"); - - // Extract trajectory data - std::vector time_arr, x_arr, y_arr, z_arr; - std::vector phi_arr, theta_arr, psi_arr; - std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - time_arr.reserve(X_sol.size()); - x_arr.reserve(X_sol.size()); - y_arr.reserve(X_sol.size()); - z_arr.reserve(X_sol.size()); - phi_arr.reserve(X_sol.size()); - theta_arr.reserve(X_sol.size()); - psi_arr.reserve(X_sol.size()); - - qw_arr.reserve(X_sol.size()); - qx_arr.reserve(X_sol.size()); - qy_arr.reserve(X_sol.size()); - qz_arr.reserve(X_sol.size()); - q_norm_arr.reserve(X_sol.size()); - - for (size_t i = 0; i < X_sol.size(); ++i) - { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - - double qw = X_sol[i](3); - double qx = X_sol[i](4); - double qy = X_sol[i](5); - double qz = X_sol[i](6); - - // Euler angles - Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - phi_arr.push_back(euler(0)); - theta_arr.push_back(euler(1)); - psi_arr.push_back(euler(2)); - - // Quaternion data - qw_arr.push_back(qw); - qx_arr.push_back(qx); - qy_arr.push_back(qy); - qz_arr.push_back(qz); - q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - } - - // Control data - std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - std::vector f1_arr, f2_arr, f3_arr, f4_arr; - f1_arr.reserve(U_sol.size()); - f2_arr.reserve(U_sol.size()); - f3_arr.reserve(U_sol.size()); - f4_arr.reserve(U_sol.size()); - - for (const auto &u : U_sol) - { - f1_arr.push_back(u(0)); - f2_arr.push_back(u(1)); - f3_arr.push_back(u(2)); - f4_arr.push_back(u(3)); + for (int i = 0; i < horizon; ++i) { + X[i + 1] = propagation_model.getDiscreteDynamics(X[i], U[i], i * timestep); } + solver.setInitialTrajectory(X, U); - // Plotting - auto f1 = figure(); - f1->size(1200, 900); // Slightly bigger if you like - - // (1) Position - auto ax1_f1 = subplot(4, 1, 0); - auto plot_handle1 = plot(ax1_f1, time_arr, x_arr, "-r"); - plot_handle1->display_name("x"); - plot_handle1->line_width(2); - hold(ax1_f1, true); - auto plot_handle2 = plot(ax1_f1, time_arr, y_arr, "-g"); - plot_handle2->display_name("y"); - plot_handle2->line_width(2); - auto plot_handle3 = plot(ax1_f1, time_arr, z_arr, "-b"); - plot_handle3->display_name("z"); - plot_handle3->line_width(2); - - title(ax1_f1, "Position Trajectories"); - xlabel(ax1_f1, "Time [s]"); - ylabel(ax1_f1, "Position [m]"); - matplot::legend(ax1_f1); - grid(ax1_f1, true); - - // (2) Attitude (Euler angles) - auto ax2_f1 = subplot(4, 1, 1); - hold(ax2_f1, true); - auto plot_handle4 = plot(ax2_f1, time_arr, phi_arr, "-r"); - plot_handle4->display_name("roll"); - plot_handle4->line_width(2); - auto plot_handle5 = plot(ax2_f1, time_arr, theta_arr, "-g"); - plot_handle5->display_name("pitch"); - plot_handle5->line_width(2); - auto plot_handle6 = plot(ax2_f1, time_arr, psi_arr, "-b"); - plot_handle6->display_name("yaw"); - plot_handle6->line_width(2); - - title(ax2_f1, "Attitude Angles"); - xlabel(ax2_f1, "Time [s]"); - ylabel(ax2_f1, "Angle [rad]"); - matplot::legend(ax2_f1); - grid(ax2_f1, true); - - // (3) Motor forces - auto ax3_f1 = subplot(4, 1, 2); - auto plot_handle7 = plot(ax3_f1, time_arr2, f1_arr, "-r"); - plot_handle7->display_name("f1"); - plot_handle7->line_width(2); - hold(ax3_f1, true); - auto plot_handle8 = plot(ax3_f1, time_arr2, f2_arr, "-g"); - plot_handle8->display_name("f2"); - plot_handle8->line_width(2); - auto plot_handle9 = plot(ax3_f1, time_arr2, f3_arr, "-b"); - plot_handle9->display_name("f3"); - plot_handle9->line_width(2); - auto plot_handle10 = plot(ax3_f1, time_arr2, f4_arr, "-k"); - plot_handle10->display_name("f4"); - plot_handle10->line_width(2); - - title(ax3_f1, "Motor Forces"); - xlabel(ax3_f1, "Time [s]"); - ylabel(ax3_f1, "Force [N]"); - matplot::legend(ax3_f1); - grid(ax3_f1, true); - - // (4) Quaternion trajectories and norm - auto ax4_f1 = subplot(4, 1, 3); - hold(ax4_f1, true); - - auto qwh = plot(ax4_f1, time_arr, qw_arr, "-r"); - qwh->display_name("q_w"); - qwh->line_width(2); - - auto qxh = plot(ax4_f1, time_arr, qx_arr, "-g"); - qxh->display_name("q_x"); - qxh->line_width(2); - - auto qyh = plot(ax4_f1, time_arr, qy_arr, "-b"); - qyh->display_name("q_y"); - qyh->line_width(2); - - auto qzh = plot(ax4_f1, time_arr, qz_arr, "-m"); - qzh->display_name("q_z"); - qzh->line_width(2); - - // Norm in black - auto qnorm_handle = plot(ax4_f1, time_arr, q_norm_arr, "-k"); - qnorm_handle->display_name("|q|"); - - title(ax4_f1, "Quaternion Components and Norm"); - xlabel(ax4_f1, "Time [s]"); - ylabel(ax4_f1, "Value"); - matplot::legend(ax4_f1); - grid(ax4_f1, true); - - // Save figure 1 - f1->draw(); - f1->save(plotDirectory + "/quadrotor_point_states.png"); - - // 3D Trajectory - auto f2 = figure(); - f2->size(800, 600); - auto ax2 = f2->current_axes(); - hold(ax2, true); - auto traj3d = plot3(ax2, x_arr, y_arr, z_arr); - traj3d->display_name("Trajectory"); - traj3d->line_style("-"); - traj3d->line_width(2); - traj3d->color("blue"); - // Project trajectory onto x-y plane at z=0 - auto proj_xy = plot3(ax2, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - proj_xy->display_name("X-Y Projection"); - proj_xy->line_style("--"); - proj_xy->line_width(1); - proj_xy->color("gray"); - xlabel(ax2, "X [m]"); - ylabel(ax2, "Y [m]"); - zlabel(ax2, "Z [m]"); - xlim(ax2, {0, 5}); - ylim(ax2, {-2, 2}); - zlim(ax2, {0, 5}); - title(ax2, "3D Trajectory (Figure-8)"); - grid(ax2, true); - f2->draw(); - f2->save(plotDirectory + "/quadrotor_point_3d.png"); - - // Animation of the quadrotor frame - auto f_anim = figure(); - f_anim->size(800, 600); - auto ax_anim = f_anim->current_axes(); - - // For collecting the trajectory as we go - std::vector anim_x, anim_y, anim_z; - anim_x.reserve(X_sol.size()); - anim_y.reserve(X_sol.size()); - anim_z.reserve(X_sol.size()); - - // Render every Nth frame to reduce #images - int frame_stride = 15; - double prop_radius = 0.03; // radius for small spheres at motor ends - - for (size_t i = 0; i < X_sol.size(); i += frame_stride) - { - ax_anim->clear(); - ax_anim->hold(true); - ax_anim->grid(true); - - // Current state - double x = X_sol[i](0); - double y = X_sol[i](1); - double z = X_sol[i](2); - - // Accumulate path - anim_x.push_back(x); - anim_y.push_back(y); - anim_z.push_back(z); - - // Plot the partial trajectory so far (in black dotted line) - auto path_plot = plot3(anim_x, anim_y, anim_z); - path_plot->line_width(1.5); - path_plot->line_style("--"); - path_plot->color("black"); - - // Build rotation from quaternion - Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // Arm endpoints (front, right, back, left) - std::vector arm_endpoints; - arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // Transform to world coords - for (auto &pt : arm_endpoints) - { - pt = Eigen::Vector3d(x, y, z) + R * pt; - } - - // Front-back arm - std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - auto fb_arm = plot3(fx, fy, fz); - fb_arm->line_width(2.0); - fb_arm->color("blue"); - - // Right-left arm - std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - auto rl_arm = plot3(rx, ry, rz); - rl_arm->line_width(2.0); - rl_arm->color("red"); - - auto sphere_points = linspace(0, 2 * M_PI, 15); - for (const auto &motor_pos : arm_endpoints) - { - std::vector circ_x, circ_y, circ_z; - circ_x.reserve(sphere_points.size()); - circ_y.reserve(sphere_points.size()); - circ_z.reserve(sphere_points.size()); - for (auto angle : sphere_points) - { - circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - circ_z.push_back(motor_pos.z()); // keep the same z for a small ring - } - auto sphere_plot = plot3(circ_x, circ_y, circ_z); - sphere_plot->line_style("solid"); - sphere_plot->line_width(1.5); - sphere_plot->color("cyan"); - } - - title(ax_anim, "Quadrotor Animation"); - xlabel(ax_anim, "X [m]"); - ylabel(ax_anim, "Y [m]"); - zlabel(ax_anim, "Z [m]"); - xlim(ax_anim, {0, 5}); - ylim(ax_anim, {-5, 5}); - zlim(ax_anim, {0, 5}); - - ax_anim->view(30, -30); - - std::string frameFile = plotDirectory + "/quadrotor_anim_frame_" + std::to_string(i / frame_stride) + ".png"; - f_anim->draw(); - f_anim->save(frameFile); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); + const cddp::CDDPSolution solution = solver.solve(cddp::SolverType::IPDDP); + if (solution.state_trajectory.empty()) { + std::cerr << "Quadrotor example failed: empty trajectory" << std::endl; + return 1; } - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_anim_frame_*.png " + plotDirectory + "/quadrotor_point.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_anim_frame_*.png"; - std::system(cleanup_command.c_str()); + const Eigen::VectorXd final_error = solution.state_trajectory.back() - goal_state; + std::cout << "Quadrotor example completed with status: " + << solution.status_message << '\n' + << "Final objective: " << solution.final_objective << '\n' + << "Final position error norm: " << final_error.head(3).norm() + << std::endl; return 0; } - -// To create a gif from the saved frames, use ImageMagick (for example): -// convert -delay 5 ../results/tests/quadrotor_frame_*.png ../results/tests/quadrotor.gif diff --git a/examples/cddp_spacecraft_linear_docking.cpp b/examples/cddp_spacecraft_linear_docking.cpp deleted file mode 100644 index 85addef8..00000000 --- a/examples/cddp_spacecraft_linear_docking.cpp +++ /dev/null @@ -1,350 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -namespace fs = std::filesystem; -using namespace cddp; - -int main() { - // ========================================================================= - // 1) Parameters for Trajectory Optimization - // ========================================================================= - - // Optimization horizon info - int N = 400; // Optimization horizon length - double time_horizon = 400.0; // Time horizon for optimization [s] - double dt = time_horizon / N; // Time step for optimization - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - - // Initial state - Eigen::VectorXd initial_state(6); - initial_state << 25.0, 25.0 / std::sqrt(3.0)-0.1, 0.0, 0.0, 0.0, 0.0; - - // Final (reference/goal) state - Eigen::VectorXd goal_state(6); - goal_state.setZero(); // Goal is the origin - - // Input constraints - double u_max = 1.0; // for each dimension - double u_min = -1.0; // for each dimension - - // Cost weighting - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(6,6); - Q.diagonal() << 1e+1, 1e+1, 1e+1, 1e-0, 1e-0, 1e-0; - - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(3,3); - R.diagonal() << 1e-0, 1e-0, 1e-0; - - // Terminal cost - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(6,6); - // Qf.diagonal() << 1e3, 1e3, 1e3, 1e1, 1e1, 1e1; - - // ========================================================================= - // 2) Setup Solver and Solve the Optimization Problem Once - // ========================================================================= - - // Create the HCW system for optimization - std::unique_ptr hcw_system = - std::make_unique(dt, mean_motion, mass, "euler"); - - // Build cost objective - std::vector empty_reference; // No intermediate reference states needed - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference, dt - ); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 500; // May need more iterations for one-shot solve - options.line_search.max_iterations = 21; - options.tolerance = 1e-5; // Tighter tolerance for final solve - options.verbose = true; // Show solver progress - options.enable_parallel = false; - options.num_threads = 8; - options.regularization.initial_value = 1e-5; - options.ipddp.barrier.mu_initial = 1e-1; // Starting barrier coefficient - - // ========================================================================= - // 2.1) Solve Unconstrained Problem for Initial Guess - // ========================================================================= - std::cout << "Solving unconstrained problem for initial guess using iLQR..." << std::endl; - - // Create a separate system and objective for the initial solve (or clone) - std::unique_ptr hcw_system_init = - std::make_unique(dt, mean_motion, mass, "euler"); - std::vector empty_reference_init; // No intermediate reference states needed - auto objective_init = std::make_unique( - Q, R, Qf, goal_state, empty_reference_init, dt - ); - - // Use simpler options for the initial guess solve - cddp::CDDPOptions options_init = options; // Copy base options - options_init.max_iterations = 100; // Fewer iterations might suffice - options_init.tolerance = 1e-4; // Looser tolerance - options_init.verbose = false; // Less verbose for initial solve - options_init.regularization.initial_value = 1e-4; - - cddp::CDDP cddp_solver_init(/*initial_state=*/initial_state, - /*goal_state=*/goal_state, - /*horizon=*/N, - /*timestep=*/dt, - /*system=*/std::move(hcw_system_init), - /*objective=*/std::move(objective_init), - /*options=*/options_init); - - // Simple linear interpolation guess for the initial solve - std::vector X_linear_init(N + 1, Eigen::VectorXd::Zero(6)); - std::vector U_zero_init(N, Eigen::VectorXd::Zero(3)); - for (int k = 0; k < N + 1; ++k) { - X_linear_init[k] = initial_state + (goal_state - initial_state) * (double(k) / N); - } - cddp_solver_init.setInitialTrajectory(X_linear_init, U_zero_init); - - // Solve the unconstrained problem (e.g., using IPDDP) - cddp::CDDPSolution initial_solution = cddp_solver_init.solve("IPDDP"); - - const auto& initial_states = initial_solution.state_trajectory; - if (initial_states.empty()) { - std::cerr << "Failed to find an initial guess solution. Exiting." << std::endl; - return 1; - } - - // ========================================================================= - // 2.2) Setup Constrained Solver (IPDDP) and Solve - // ========================================================================= - std::cout << "Setting up and solving the constrained problem using IPDDP..." << std::endl; - - // Setup CDDP solver instance (using IPDDP) - cddp::CDDP cddp_solver(/*initial_state=*/initial_state, - /*goal_state=*/goal_state, // Pass goal state (used by objective) - /*horizon=*/N, - /*timestep=*/dt, - /*system=*/std::move(hcw_system), // System ownership transferred - /*objective=*/std::move(objective), // Objective ownership transferred - /*options=*/options); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // Add Linear Constraints for X-Y Plane Cone Boundary (|y| <= x * tan(fov) for x >= 0) - // Constraint 1: -tan(fov)*x + y <= 0 - // Constraint 2: -tan(fov)*x - y <= 0 - Eigen::Vector3d origin(0.0, 0.0, 0.0); - Eigen::Vector3d axis(1.0, 0.0, 0.0); // Opening along positive X-axis - double fov = M_PI / 3.0; // 30 degrees half-angle - double tan_fov = std::tan(fov); - // Eigen::MatrixXd A_cone_xy(2, 6); - // A_cone_xy << -tan_fov, 1.0, 0.0, 0.0, 0.0, 0.0, // Row 1 - // -tan_fov, -1.0, 0.0, 0.0, 0.0, 0.0; // Row 2 - // Eigen::VectorXd b_cone_xy = Eigen::VectorXd::Zero(2); - // cddp_solver.addConstraint("ConeXYLinearConstraint", - // std::make_unique(A_cone_xy, b_cone_xy)); - - // Add Second Order Cone Constraint - cddp_solver.addPathConstraint("SecondOrderConeConstraint", - std::make_unique(origin, axis, fov, 1e-6)); - - // Initialize trajectory guess - // Use the solution from the unconstrained solve as the initial guess - const auto& X_init = initial_solution.state_trajectory; - const auto& U_init = initial_solution.control_trajectory; - - // Assign the initial trajectory guess to the solver - cddp_solver.setInitialTrajectory(X_init, U_init); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - - - // ========================================================================= - // 3) Visualize the Trajectory - // ========================================================================= - const auto& solution_states = solution.state_trajectory; - if (!solution_states.empty()) { - namespace plt = matplot; - - std::vector x_traj, y_traj, z_traj; - for (const auto& state : solution_states) { - if (state.size() >= 3) { // Ensure state has at least 3 dimensions (x, y, z) - x_traj.push_back(state(0)); - y_traj.push_back(state(1)); - z_traj.push_back(state(2)); - } - } - - auto fig = plt::figure(); - plt::plot3(x_traj, y_traj, z_traj, "-o")->line_width(2).marker_size(4); - plt::hold(true); - - // Plot start and end points - plt::plot3(std::vector{initial_state(0)}, std::vector{initial_state(1)}, std::vector{initial_state(2)}, "go") - ->marker_size(10).display_name("Start"); - plt::plot3(std::vector{goal_state(0)}, std::vector{goal_state(1)}, std::vector{goal_state(2)}, "rx") - ->marker_size(10).display_name("Goal"); - - plt::xlabel("X [m]"); - plt::ylabel("Y [m]"); - plt::zlabel("Z [m]"); - plt::title("Spacecraft Docking Trajectory"); - - // --- Add Cone Visualization (3D) --- - // Generate cone surface points (opening along +X) - double cone_x_min_vis = -5.0; // Min x-extent for visualization - double cone_x_max_vis = goal_state(0) + 5.0; // Max x-extent (a bit beyond goal) - if (!y_traj.empty()) { - // Adjust max based on trajectory, but min should start at origin for +Y side visualization - cone_x_max_vis = std::max(cone_x_max_vis, *std::max_element(x_traj.begin(), x_traj.end()) + 5.0); - } - // Start cone visualization from the origin's x-coordinate (for +X side) - double cone_x_start = origin(0); - cone_x_max_vis = std::max(cone_x_max_vis, cone_x_start + 1.0); // Ensure some minimum length - - std::vector x_cone_vals = plt::linspace(cone_x_start, cone_x_max_vis, 30); - std::vector theta_vals = plt::linspace(0, 2 * M_PI, 50); - auto [X_cone_grid, THETA_cone] = plt::meshgrid(x_cone_vals, theta_vals); // Grid X values - - // Find orthogonal vectors u, v to the axis (now X-axis) - Eigen::Vector3d u = Eigen::Vector3d(0.0, 1.0, 0.0); // Y-direction - Eigen::Vector3d v_ortho = Eigen::Vector3d(0.0, 0.0, 1.0); // Z-direction - - // Calculate Y, Z coordinates for the cone surface - std::vector> Y_cone(X_cone_grid.size(), std::vector(X_cone_grid[0].size())); - std::vector> Z_cone(X_cone_grid.size(), std::vector(X_cone_grid[0].size())); - - for (size_t i = 0; i < X_cone_grid.size(); ++i) { - for (size_t j = 0; j < X_cone_grid[0].size(); ++j) { - // Cone opening along positive X: radius depends on displacement from origin along axis - double x_disp = X_cone_grid[i][j] - origin(0); // x_disp will be >= 0 due to linspace start - - // Radius calculation based on positive displacement along the axis - double radius = x_disp * tan_fov; // No abs needed since x_disp >= 0 - double theta = THETA_cone[i][j]; - Eigen::Vector3d point_on_circle = radius * (std::cos(theta) * u + std::sin(theta) * v_ortho); - Eigen::Vector3d point = origin + axis * x_disp + point_on_circle; // Point relative to origin based on x_disp - // X_cone_grid[i][j] already holds the x value - Y_cone[i][j] = point.y(); - Z_cone[i][j] = point.z(); - } - } - - // Plot the cone surface - plt::surf(X_cone_grid, Y_cone, Z_cone)->face_alpha(0.3).edge_color("none").display_name("FOV Cone (+X)"); - // --- End Cone Visualization (3D) --- - - plt::legend(); - plt::grid(true); - plt::axis("equal"); // Keep aspect ratio for better visualization - plt::hold(false); - plt::show(); - - // Define plot directory and create if it doesn't exist - std::string plotDirectory = "../results/tests"; // Assuming run from build dir - if (!fs::exists(plotDirectory)) { - try { - fs::create_directories(plotDirectory); - } catch (const std::filesystem::filesystem_error& e) { - std::cerr << "Error creating directory \"" << plotDirectory << "\": " << e.what() << std::endl; - // Optionally handle the error, e.g., save to current dir instead - plotDirectory = "."; // Fallback to current directory - } - } - - std::string filename = plotDirectory + "/docking_trajectory.png"; - try { - plt::save(filename); - std::cout << "Saved trajectory visualization to " << filename << std::endl; - } catch (const std::runtime_error& e) { - std::cerr << "Error saving plot to \"" << filename << "\": " << e.what() << std::endl; - } - - // ========================================================================= - // 4) Visualize the Trajectory (X-Y Plane) - // ========================================================================= - auto fig_xy = plt::figure(); - plt::plot(x_traj, y_traj, "-o")->line_width(2).marker_size(4); - plt::hold(true); - - // Plot start and end points (2D) - plt::plot({initial_state(0)}, {initial_state(1)}, "go")->marker_size(10).display_name("Start"); - plt::plot({goal_state(0)}, {goal_state(1)}, "rx")->marker_size(10).display_name("Goal"); - - plt::xlabel("X [m]"); - plt::ylabel("Y [m]"); - plt::title("Spacecraft Docking Trajectory (X-Y Plane)"); - - // --- Add Cone Visualization (2D - X/Y Slice at Z=0) --- - // For cone opening along X-axis: Boundary lines are y = oy +/- (x - ox) * tan_fov - auto current_xlim = plt::xlim(); - // Use only the positive side x >= ox for the lines, consistent with 3D plot - double x_line_start = std::max(current_xlim[0], origin(0)); - double x_line_end = current_xlim[1]; - if (x_line_end <= x_line_start) { // Ensure valid range if xlim is weird - x_line_end = x_line_start + 1.0; - } - std::vector x_line_vals = {x_line_start, x_line_end}; - - // Adjust x values relative to origin for radius calculation - std::vector x_rel_origin = {x_line_vals[0] - origin(0), x_line_vals[1] - origin(0)}; - - // Line 1: y = oy + (x - ox) * tan_fov - std::vector y_line1 = {origin(1) + x_rel_origin[0] * tan_fov, origin(1) + x_rel_origin[1] * tan_fov}; - // Line 2: y = oy - (x - ox) * tan_fov - std::vector y_line2 = {origin(1) - x_rel_origin[0] * tan_fov, origin(1) - x_rel_origin[1] * tan_fov}; - - plt::plot(x_line_vals, y_line1, "k--")->line_width(1.5).display_name("Cone Boundary (X-Y)"); - plt::plot(x_line_vals, y_line2, "k--")->line_width(1.5); - // Ensure plot limits encompass the lines if needed (axis equal might handle it) - plt::xlim(current_xlim); // Restore xlim just in case plot adjusted it - // --- End Cone Visualization (2D) --- - - plt::legend(); - plt::grid(true); - plt::axis("equal"); - plt::xlim({-10.0, 30.0}); - plt::ylim({-10.0, 30.0}); - plt::hold(false); - plt::show(); - - std::string filename_xy = plotDirectory + "/docking_trajectory_xy.png"; - try { - plt::save(fig_xy, filename_xy); // Save the specific figure - std::cout << "Saved X-Y trajectory visualization to " << filename_xy << std::endl; - } catch (const std::runtime_error& e) { - std::cerr << "Error saving X-Y plot to \"" << filename_xy << "\": " << e.what() << std::endl; - } - - } else { - std::cerr << "Solution trajectory is empty, cannot visualize." << std::endl; - } - - - std::cout << "Trajectory optimization finished." << std::endl; - return 0; -} \ No newline at end of file diff --git a/examples/cddp_spacecraft_linear_rpo.cpp b/examples/cddp_spacecraft_linear_rpo.cpp deleted file mode 100644 index 5cb94a60..00000000 --- a/examples/cddp_spacecraft_linear_rpo.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -namespace cddp -{ - class SumOfTwoNormObjective : public NonlinearObjective - { - public: - SumOfTwoNormObjective(const Eigen::VectorXd &goal_state, - double weight_running_control, - double weight_terminal_state, - double timestep) - : NonlinearObjective(timestep), - goal_state_(goal_state), - weight_running_control_(weight_running_control), - weight_terminal_state_(weight_terminal_state) - { - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int /*index*/) const override - { - double control_squared = control.squaredNorm(); - double control_norm = std::sqrt(control_squared + epsilon_); - return weight_running_control_ * control_norm; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return weight_terminal_state_ * state_error.squaredNorm(); - } - - // Analytical Gradients - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state.size()); // Or handle error appropriately if size unknown - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - if (norm_u < 1e-8) - { - return Eigen::VectorXd::Zero(control.size()); - } - return weight_running_control_ * control / norm_u; - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return 2.0 * weight_terminal_state_ * state_error; - } - - // Analytical Hessians - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state.size(), state.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - int control_dim = control.size(); - if (norm_u < 1e-8) - { - return weight_running_control_ * Eigen::MatrixXd::Identity(control_dim, control_dim) / 1e-8; // or a small value like 1.0 - } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd u_ut = control * control.transpose(); - return weight_running_control_ * (I / norm_u - u_ut / (norm_u * norm_u * norm_u)); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd & /*final_state*/) const override - { - int state_dim = goal_state_.size(); - return 2.0 * weight_terminal_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim); - } - - private: - Eigen::VectorXd goal_state_; - double weight_running_control_; - double weight_terminal_state_; - double epsilon_ = 1e-5; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Optimization horizon info - int horizon = 500; // Optimization horizon length - double time_horizon = 10000.0; // Time horizon for optimization [s] - double dt = time_horizon / horizon; // Time step for optimization - int state_dim = 6; - int control_dim = 3; - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - - // Initial state (v-bar hold at 1km) - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, -1000.0, 0.0, 0.0, 0.0, 0.0; - - // Final (reference/goal) state (r-bar ) - Eigen::VectorXd goal_state(state_dim); - goal_state << -100.0, 0.0, 0.0, 0.0, 2*mean_motion*100.0, 0.0; - - // Input constraints - double u_max = 0.05; // for each dimension - double u_min = -0.05; // for each dimension - double u_min_norm = 0.0; // Minimum thrust magnitude - double u_max_norm = 0.05; // Maximum thrust magnitude, consistent with previous u_max - - // Cost weighting for SumOfTwoNormObjective - double weight_running_control = 1.0; // Example value - double weight_terminal_state = 1000.0; // Example value - - // Create the HCW system for optimization - std::unique_ptr hcw_system = - std::make_unique(dt, mean_motion, mass, "euler"); - - // Build cost objective - auto objective = std::make_unique( - goal_state, - weight_running_control, - weight_terminal_state, - dt); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 1000; // May need more iterations for one-shot solve - options.line_search.max_iterations = 21; - options.tolerance = 1e-7; // Tighter tolerance for final solve - options.verbose = true; // Show solver progress - options.enable_parallel = false; - options.num_threads = 8; - // Regularization type is now implicit in new API - options.regularization.initial_value = 1e-3; - options.ipddp.barrier.mu_initial = 1e-0; // Starting barrier coefficient - options.use_ilqr = true; - options.debug = false; - options.msipddp.segment_length = horizon; - options.msipddp.rollout_type = "nonlinear"; - - // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - // Generate initial trajectory by constant initial state - for (int i = 0; i < horizon; ++i) - { - // X[i + 1] = hcw_system->getDiscreteDynamics(X[i], U[i]); - X[i+1] = initial_state; - } - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - dt, - std::move(hcw_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // // Add Thrust Magnitude Constraint - // cddp_solver.addConstraint("MaxThrustMagnitudeConstraint", - // std::make_unique(u_max_norm)); - - // Add Ball Constraint (for keep-out zone) - double radius = 90.0; - Eigen::Vector2d center(0.0, 0.0); - // cddp_solver.addConstraint("BallConstraint", - // std::make_unique(radius, center, 0.1)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract the solution - const auto& X_solution = solution.state_trajectory; - const auto& U_solution = solution.control_trajectory; - - if (!X_solution.empty() && !U_solution.empty()) - { - namespace plt = matplot; - - // Create time vectors - std::vector t_states(horizon + 1); - for(int i = 0; i <= horizon; ++i) t_states[i] = i * dt; - - std::vector t_controls(horizon); - for(int i = 0; i < horizon; ++i) t_controls[i] = i * dt; - - // Extract individual state and control trajectories - std::vector x_pos(horizon + 1), y_pos(horizon + 1), z_pos(horizon + 1); - std::vector vx(horizon + 1), vy(horizon + 1), vz(horizon + 1); - - for (size_t i = 0; i < X_solution.size(); ++i) { - const auto& state = X_solution[i]; - if (state.size() >= state_dim) { // Ensure state has at least 8 dimensions - x_pos[i] = state(0); - y_pos[i] = state(1); - z_pos[i] = state(2); - vx[i] = state(3); - vy[i] = state(4); - vz[i] = state(5); - } - } - - std::vector ux(horizon), uy(horizon), uz(horizon); - std::vector thrust_magnitude(horizon); - - for (size_t i = 0; i < U_solution.size(); ++i) { - const auto& control = U_solution[i]; - if (control.size() >= control_dim) { // Ensure control has at least 3 dimensions - ux[i] = control(0); - uy[i] = control(1); - uz[i] = control(2); - thrust_magnitude[i] = control.norm(); - } - } - - // Circle data for plotting - std::vector circle_x, circle_y; - for (double theta = 0; theta <= 2*M_PI; theta += 0.01) { - circle_x.push_back(radius * std::cos(theta)); - circle_y.push_back(radius * std::sin(theta)); - } - - // --- Generate Plots --- - // plt::figure_size(1200, 800); // Removed due to compilation error - - // 1. Position Trajectories - plt::figure(); - plt::plot(t_states, x_pos)->line_width(2).display_name("x (pos)"); - plt::hold(true); - plt::plot(t_states, y_pos)->line_width(2).display_name("y (pos)"); - plt::plot(t_states, z_pos)->line_width(2).display_name("z (pos)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("Position vs. Time"); - - // 2. Velocity Trajectories - plt::figure(); - plt::plot(t_states, vx)->line_width(2).display_name("vx"); - plt::hold(true); - plt::plot(t_states, vy)->line_width(2).display_name("vy"); - plt::plot(t_states, vz)->line_width(2).display_name("vz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("Velocity vs. Time"); - - // 5. Control Inputs (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, ux)->line_width(2).display_name("ux"); - plt::hold(true); - plt::stairs(t_controls, uy)->line_width(2).display_name("uy"); - plt::stairs(t_controls, uz)->line_width(2).display_name("uz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Input"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time"); - - // 6. Thrust Magnitude (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, thrust_magnitude)->line_width(2).display_name("||Thrust|| (ZOH)"); - // Add norm constraint lines if available - // plt::hold(true); - // plt::plot(t_controls, std::vector(horizon, u_max_norm), "--r")->display_name("Max Norm"); - // if (u_min_norm > 0) plt::plot(t_controls, std::vector(horizon, u_min_norm), "--y")->display_name("Min Norm"); - // plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Thrust Magnitude"); - plt::legend(); - plt::title("Thrust Magnitude (ZOH) vs. Time"); - - // 7. X-Y plane trajectory (x-axis vertical to the top, y-axis horizontal to the left) - plt::figure(); - plt::plot(y_pos, x_pos)->line_width(2).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty() && !y_pos.empty()){ // Check if trajectories are not empty - plt::scatter(std::vector{y_pos.front()}, std::vector{x_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter(std::vector{y_pos.back()}, std::vector{x_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - // Plot Ball Constraint - plt::hold(true); - plt::plot(circle_x, circle_y)->line_width(2).display_name("Ball Constraint"); - plt::hold(false); - plt::xlabel("y (m)"); - plt::ylabel("x (m)"); - plt::legend(); - plt::title("X-Y Plane Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - plt::gca()->x_axis().reverse(true); // Make y-axis (horizontal on plot) increase to the left - - // 8. 3D Trajectory - plt::figure(); - plt::plot3(x_pos, y_pos, z_pos, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty()){ // Check if trajectories are not empty - plt::scatter3(std::vector{x_pos.front()}, std::vector{y_pos.front()}, std::vector{z_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter3(std::vector{x_pos.back()}, std::vector{y_pos.back()}, std::vector{z_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - plt::hold(false); - plt::xlabel("x (m)"); - plt::ylabel("y (m)"); - plt::zlabel("z (m)"); - plt::legend(); - plt::title("3D Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - - // Show all plots - plt::show(); - std::cout << "Plotting complete." << std::endl; - } - else - { - std::cout << "Solver did not find a solution, or solution variables are not available. Skipping plots." << std::endl; - } - return 0; - } \ No newline at end of file diff --git a/examples/cddp_spacecraft_nonlinear_rpo.cpp b/examples/cddp_spacecraft_nonlinear_rpo.cpp deleted file mode 100644 index e727519a..00000000 --- a/examples/cddp_spacecraft_nonlinear_rpo.cpp +++ /dev/null @@ -1,383 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -namespace cddp -{ - class SumOfTwoNormObjective : public NonlinearObjective - { - public: - SumOfTwoNormObjective(const Eigen::VectorXd &goal_state, - double weight_running_control, - double weight_terminal_state, - double timestep) - : NonlinearObjective(timestep), - goal_state_(goal_state), - weight_running_control_(weight_running_control), - weight_terminal_state_(weight_terminal_state) - { - Qf_ = Eigen::MatrixXd::Identity(goal_state.size(), goal_state.size()); - Qf_(6, 6) = 0.0; - Qf_(7, 7) = 0.0; - Qf_(8, 8) = 0.0; - Qf_(9, 9) = 0.0; - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int /*index*/) const override - { - double control_squared = control.squaredNorm(); - double control_norm = std::sqrt(control_squared + epsilon_); - return weight_running_control_ * control_norm; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return weight_terminal_state_ * state_error.transpose() * Qf_ * state_error; - } - - // Analytical Gradients - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state.size()); // Or handle error appropriately if size unknown - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - if (norm_u < 1e-8) - { - return Eigen::VectorXd::Zero(control.size()); - } - return weight_running_control_ * control / norm_u; - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return 2.0 * weight_terminal_state_ * Qf_ * state_error; - } - - // Analytical Hessians - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state.size(), state.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - int control_dim = control.size(); - if (norm_u < 1e-8) - { - return weight_running_control_ * Eigen::MatrixXd::Identity(control_dim, control_dim) / 1e-8; // or a small value like 1.0 - } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd u_ut = control * control.transpose(); - return weight_running_control_ * (I / norm_u - u_ut / (norm_u * norm_u * norm_u)); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd & /*final_state*/) const override - { - int state_dim = goal_state_.size(); - return 2.0 * weight_terminal_state_ * Qf_; - } - - private: - Eigen::VectorXd goal_state_; - double weight_running_control_; - double weight_terminal_state_; - double epsilon_ = 1e-5; - Eigen::MatrixXd Qf_; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Optimization horizon info - int horizon = 500; // Optimization horizon length - double time_horizon = 10000.0; // Time horizon for optimization [s] - double dt = time_horizon / horizon; // Time step for optimization - int state_dim = 10; - int control_dim = 3; - - // HCW parameters - double gravitational_parameter = 3.9860044e14; - double ref_radius = (6371.0 + 500.0) * 1e3; - double ref_period = 2 * M_PI * sqrt(pow(ref_radius, 3) / 3.9860044e14); - double ref_mean_motion = 2 * M_PI / ref_period; - double mass = 100.0; - double nominal_radius = 50.0; - std::string integration_type = "rk4"; - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, -1000.0, 0.0, 0.0, 0.0, 0.0, ref_radius, 0.0, 0.0, ref_mean_motion; - - // Final (reference/goal) state - Eigen::VectorXd goal_state(state_dim); - goal_state << -100.0, 0.0, 0.0, 0.0, 2*ref_mean_motion*100.0, 0.0, ref_radius, 0.0, 0.0, ref_mean_motion; - - // Input constraints - double u_max = 0.05; // for each dimension - double u_min = -0.05; // for each dimension - double u_min_norm = 0.0; // Minimum thrust magnitude - double u_max_norm = 0.05; // Maximum thrust magnitude, consistent with previous u_max - - // Cost weighting for SumOfTwoNormObjective - double weight_running_control = 1.0; // Example value - double weight_terminal_state = 1000.0; // Example value - - // Create the SpacecraftNonlinear system for optimization - std::unique_ptr spacecraft_system = - std::make_unique(dt, integration_type, mass, 1.0, 1.0, gravitational_parameter); - - // Build cost objective - auto objective = std::make_unique( - goal_state, - weight_running_control, - weight_terminal_state, - dt); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 100; // May need more iterations for one-shot solve - options.line_search.max_iterations = 21; - options.tolerance = 1e-7; // Tighter tolerance for final solve - options.verbose = true; // Show solver progress - options.enable_parallel = false; - options.num_threads = 8; - // Regularization type is now implicit in new API - options.regularization.initial_value = 1e-3; - options.ipddp.barrier.mu_initial = 1e-0; // Starting barrier coefficient - options.use_ilqr = true; - options.debug = false; - options.msipddp.segment_length = horizon; - options.msipddp.rollout_type = "nonlinear"; - - // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - // Generate initial trajectory by constant initial state - for (int i = 0; i < horizon; ++i) - { - // X[i + 1] = hcw_system->getDiscreteDynamics(X[i], U[i]); - X[i+1] = initial_state; - } - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - dt, - std::move(spacecraft_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // // Add Thrust Magnitude Constraint - // cddp_solver.addConstraint("MaxThrustMagnitudeConstraint", - // std::make_unique(u_max_norm)); - - // Add Ball Constraint (for keep-out zone) - double radius = 90.0; - Eigen::Vector2d center(0.0, 0.0); - // cddp_solver.addConstraint("BallConstraint", - // std::make_unique(radius, center, 0.1)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract the solution - const auto& X_solution = solution.state_trajectory; - const auto& U_solution = solution.control_trajectory; - - if (!X_solution.empty() && !U_solution.empty()) - { - namespace plt = matplot; - - // Create time vectors - std::vector t_states(horizon + 1); - for(int i = 0; i <= horizon; ++i) t_states[i] = i * dt; - - std::vector t_controls(horizon); - for(int i = 0; i < horizon; ++i) t_controls[i] = i * dt; - - // Extract individual state and control trajectories - std::vector x_pos(horizon + 1), y_pos(horizon + 1), z_pos(horizon + 1); - std::vector vx(horizon + 1), vy(horizon + 1), vz(horizon + 1); - - for (size_t i = 0; i < X_solution.size(); ++i) { - const auto& state = X_solution[i]; - if (state.size() >= state_dim) { // Ensure state has at least 8 dimensions - x_pos[i] = state(0); - y_pos[i] = state(1); - z_pos[i] = state(2); - vx[i] = state(3); - vy[i] = state(4); - vz[i] = state(5); - } - } - - std::vector ux(horizon), uy(horizon), uz(horizon); - std::vector thrust_magnitude(horizon); - - for (size_t i = 0; i < U_solution.size(); ++i) { - const auto& control = U_solution[i]; - if (control.size() >= control_dim) { // Ensure control has at least 3 dimensions - ux[i] = control(0); - uy[i] = control(1); - uz[i] = control(2); - thrust_magnitude[i] = control.norm(); - } - } - - // Circle data for plotting - std::vector circle_x, circle_y; - for (double theta = 0; theta <= 2*M_PI; theta += 0.01) { - circle_x.push_back(radius * std::cos(theta)); - circle_y.push_back(radius * std::sin(theta)); - } - - // --- Generate Plots --- - // plt::figure_size(1200, 800); // Removed due to compilation error - - // 1. Position Trajectories - plt::figure(); - plt::plot(t_states, x_pos)->line_width(2).display_name("x (pos)"); - plt::hold(true); - plt::plot(t_states, y_pos)->line_width(2).display_name("y (pos)"); - plt::plot(t_states, z_pos)->line_width(2).display_name("z (pos)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("Position vs. Time"); - - // 2. Velocity Trajectories - plt::figure(); - plt::plot(t_states, vx)->line_width(2).display_name("vx"); - plt::hold(true); - plt::plot(t_states, vy)->line_width(2).display_name("vy"); - plt::plot(t_states, vz)->line_width(2).display_name("vz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("Velocity vs. Time"); - - // 5. Control Inputs (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, ux)->line_width(2).display_name("ux"); - plt::hold(true); - plt::stairs(t_controls, uy)->line_width(2).display_name("uy"); - plt::stairs(t_controls, uz)->line_width(2).display_name("uz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Input"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time"); - - // 6. Thrust Magnitude (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, thrust_magnitude)->line_width(2).display_name("||Thrust|| (ZOH)"); - // Add norm constraint lines if available - // plt::hold(true); - // plt::plot(t_controls, std::vector(horizon, u_max_norm), "--r")->display_name("Max Norm"); - // if (u_min_norm > 0) plt::plot(t_controls, std::vector(horizon, u_min_norm), "--y")->display_name("Min Norm"); - // plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Thrust Magnitude"); - plt::legend(); - plt::title("Thrust Magnitude (ZOH) vs. Time"); - - // 7. X-Y plane trajectory (x-axis vertical to the top, y-axis horizontal to the left) - plt::figure(); - plt::plot(y_pos, x_pos)->line_width(2).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty() && !y_pos.empty()){ // Check if trajectories are not empty - plt::scatter(std::vector{y_pos.front()}, std::vector{x_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter(std::vector{y_pos.back()}, std::vector{x_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - // Plot Ball Constraint - plt::hold(true); - plt::plot(circle_x, circle_y)->line_width(2).display_name("Ball Constraint"); - plt::hold(false); - plt::xlabel("y (m)"); - plt::ylabel("x (m)"); - plt::legend(); - plt::title("X-Y Plane Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - plt::gca()->x_axis().reverse(true); // Make y-axis (horizontal on plot) increase to the left - - // 8. 3D Trajectory - plt::figure(); - plt::plot3(x_pos, y_pos, z_pos, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty()){ // Check if trajectories are not empty - plt::scatter3(std::vector{x_pos.front()}, std::vector{y_pos.front()}, std::vector{z_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter3(std::vector{x_pos.back()}, std::vector{y_pos.back()}, std::vector{z_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - plt::hold(false); - plt::xlabel("x (m)"); - plt::ylabel("y (m)"); - plt::zlabel("z (m)"); - plt::legend(); - plt::title("3D Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - - // Show all plots - plt::show(); - std::cout << "Plotting complete." << std::endl; - } - else - { - std::cout << "Solver did not find a solution, or solution variables are not available. Skipping plots." << std::endl; - } - return 0; - } \ No newline at end of file diff --git a/examples/cddp_spacecraft_roe_rpo.cpp b/examples/cddp_spacecraft_roe_rpo.cpp deleted file mode 100644 index e35de013..00000000 --- a/examples/cddp_spacecraft_roe_rpo.cpp +++ /dev/null @@ -1,478 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "dynamics_model/spacecraft_roe.hpp" // Added for SpacecraftROE -namespace plt = matplot; - -namespace cddp -{ - class SumOfTwoNormObjective : public NonlinearObjective - { - public: - SumOfTwoNormObjective(const Eigen::VectorXd &goal_state_roe, // Renamed for clarity - double weight_running_control, - double weight_terminal_state, - double timestep, - double a_ref_m) - : NonlinearObjective(timestep), - goal_state_roe_(goal_state_roe), // Renamed for clarity - weight_running_control_(weight_running_control), - weight_terminal_state_(weight_terminal_state), - a_ref_m_(a_ref_m) - { - } - - double running_cost(const Eigen::VectorXd &state_roe, // Renamed for clarity - const Eigen::VectorXd &control, - int /*index*/) const override - { - double control_squared = control.squaredNorm(); - if (control_squared < 1e-10) - { - return 0.0; - } - double control_norm = std::sqrt(control_squared + epsilon_); - return weight_running_control_ * control_norm; - } - - double terminal_cost(const Eigen::VectorXd &final_state_roe) const override // Renamed for clarity - { - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(final_state_roe.size(), final_state_roe.size()); - Q(6, 6) = 0.0; - Eigen::VectorXd state_error = (final_state_roe - goal_state_roe_) * a_ref_m_; - return weight_terminal_state_ * state_error.transpose() * Q * state_error; - } - - // Analytical Gradients - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd &state_roe, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state_roe.size()); - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd & /*state_roe*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - if (norm_u < 1e-8) - { - return Eigen::VectorXd::Zero(control.size()); - } - return weight_running_control_ * control / norm_u; - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd &final_state_roe) const override - { - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(final_state_roe.size(), final_state_roe.size()); - Q(6, 6) = 0.0; - Eigen::VectorXd state_error = (final_state_roe - goal_state_roe_) * a_ref_m_; - return 2.0 * weight_terminal_state_ * Q * state_error; - } - - // Analytical Hessians - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd &state_roe, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state_roe.size(), state_roe.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd & /*state_roe*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - int control_dim = control.size(); - if (norm_u < 1e-8) - { - return weight_running_control_ * Eigen::MatrixXd::Identity(control_dim, control_dim) / 1e-8; // or a small value like 1.0 - } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd u_ut = control * control.transpose(); - return weight_running_control_ * (I / norm_u - u_ut / (norm_u * norm_u * norm_u)); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd &state_roe, const Eigen::VectorXd &control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state_roe.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd & /*final_state_roe*/) const override - { - int state_dim = goal_state_roe_.size(); - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(state_dim, state_dim); - Q(6, 6) = 0.0; - return 2.0 * weight_terminal_state_ * Q; - } - - private: - Eigen::VectorXd goal_state_roe_; // Stores goal state in ROE coordinates - double weight_running_control_; - double weight_terminal_state_; - double epsilon_ = 1e-6; - double a_ref_m_; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Optimization horizon info - int horizon = 500; // Optimization horizon length - double dt = 20.0; // Time step for optimization - int state_dim = SpacecraftROE::STATE_DIM; // Use consistent state dimension from SpacecraftROE - int control_dim = SpacecraftROE::CONTROL_DIM; // Use consistent control dimension - - // SpacecraftROE parameters - double a_ref_m = (6371.0 + 500.0) * 1e3; // Reference semi-major axis (m) e.g. LEO - double nu0_rad = 0.0; // Initial argument of latitude for ROE (rad) - double mass_kg = 1.0; // Mass in kg - double period = 2 * M_PI * sqrt(pow(a_ref_m, 3) / 3.9860044e14); // Orbital period (s) - double mean_motion = 2 * M_PI / period; - - // Input constraints - double u_force_max_N = 0.05; - - // Cost weighting for SumOfTwoNormObjective - double weight_running_control = 1.0; - double weight_terminal_state = 10.0; - - // Create the SpacecraftROE system for optimization - std::unique_ptr roe_system = - std::make_unique(dt, "euler", a_ref_m, nu0_rad); - - // Initial state - Eigen::VectorXd initial_state_roe(state_dim); - // initial_state_roe << 0.0, 1000.0, 0.0, 500.0, 0.0, 700.0, 0.0; // Time is the 7th element - // initial_state_roe /= a_ref_m; - // initial_state_roe(6) = 0.0; - Eigen::VectorXd initial_state_hcw(6); - initial_state_hcw << 0.0, -1000.0, 0.0, 0.0, 0.0, 0.0; - if (auto* roe_model = dynamic_cast(roe_system.get())) { - initial_state_roe = roe_model->transformHCWToROE(initial_state_hcw, 0.0); - } else { - std::cerr << "Error: roe_system is not a SpacecraftROE model at initial_state_roe." << std::endl; - return 1; // Or handle error appropriately - } - - // Final state - Eigen::VectorXd goal_state_roe(state_dim); - // goal_state_roe << 0.0, 100.0, 0.0, 300.0, 0.0, 400.0, 0.0; // Time is the 7th element, init to 0.0 - // goal_state_roe /= a_ref_m; - // goal_state_roe(6) = horizon * dt; // Set target time to the end of the horizon - Eigen::VectorXd goal_state_hcw(6); - goal_state_hcw << -100.0, 0.0, 0.0, 0.0, 200.0 * mean_motion, 0.0; - if (auto* roe_model = dynamic_cast(roe_system.get())) { - goal_state_roe = roe_model->transformHCWToROE(goal_state_hcw, horizon * dt); - } else { - std::cerr << "Error: roe_system is not a SpacecraftROE model at goal_state_roe." << std::endl; - return 1; // Or handle error appropriately - } - - // Build cost objective - auto objective = std::make_unique( - goal_state_roe, - weight_running_control, - weight_terminal_state, - dt, - a_ref_m); - - // ======= Initial and final trajectory ======= - int num_steps = static_cast(12.0 * period / dt); // Simulate for 3 orbits - // Initial and final trajectory - std::vector X_roe_initial(num_steps + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U_accel_initial(num_steps, Eigen::VectorXd::Zero(control_dim)); - X_roe_initial[0] = initial_state_roe; - for (int i = 0; i < num_steps; ++i) // Iterate up to num_steps - { - X_roe_initial[i + 1] = roe_system->getDiscreteDynamics(X_roe_initial[i], U_accel_initial[i], i * dt); - } - std::vector X_roe_final(num_steps + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U_accel_final(num_steps, Eigen::VectorXd::Zero(control_dim)); - X_roe_final[0] = goal_state_roe; - for (int i = 0; i < num_steps; ++i) // Iterate up to num_steps - { - X_roe_final[i + 1] = roe_system->getDiscreteDynamics(X_roe_final[i], U_accel_final[i], i * dt); - } - - // Convert initial and final trajectory to HCW coordinates vector of doubles - std::vector> X_hcw_initial(num_steps + 1); - std::vector> X_hcw_final(num_steps + 1); - for (int i = 0; i < num_steps + 1; ++i) - { - if (auto* roe_model = dynamic_cast(roe_system.get())) { - Eigen::VectorXd state_hcw_initial = roe_model->transformROEToHCW(X_roe_initial[i].head(6), X_roe_initial[i][6]); - Eigen::VectorXd state_hcw_final = roe_model->transformROEToHCW(X_roe_final[i].head(6), X_roe_final[i][6]); - X_hcw_initial[i] = {state_hcw_initial(0), state_hcw_initial(1), state_hcw_initial(2), state_hcw_initial(3), state_hcw_initial(4), state_hcw_initial(5)}; - X_hcw_final[i] = {state_hcw_final(0), state_hcw_final(1), state_hcw_final(2), state_hcw_final(3), state_hcw_final(4), state_hcw_final(5)}; - } else { - std::cerr << "Error: roe_system is not a SpacecraftROE model during trajectory conversion." << std::endl; - // Handle error appropriately, maybe clear X_hcw_initial[i] or return - } - } - - // Prepare data for 3D plotting - std::vector initial_x_hcw(num_steps + 1), initial_y_hcw(num_steps + 1), initial_z_hcw(num_steps + 1); - std::vector initial_t(num_steps + 1); - std::vector final_x_hcw(num_steps + 1), final_y_hcw(num_steps + 1), final_z_hcw(num_steps + 1); - std::vector final_t(num_steps + 1); - - for (int i = 0; i < num_steps + 1; ++i) { - if (X_hcw_initial[i].size() >= 3) { // Ensure there are enough elements - initial_x_hcw[i] = X_hcw_initial[i][0]; - initial_y_hcw[i] = X_hcw_initial[i][1]; - initial_z_hcw[i] = X_hcw_initial[i][2]; - initial_t[i] = X_roe_initial[i][6]; - } - if (X_hcw_final[i].size() >= 3) { // Ensure there are enough elements - final_x_hcw[i] = X_hcw_final[i][0]; - final_y_hcw[i] = X_hcw_final[i][1]; - final_z_hcw[i] = X_hcw_final[i][2]; - final_t[i] = X_roe_final[i][6]; - } - } - // ======= End of Initial and final trajectory ======= - - // // // Plot 3d initial and final trajectory - // plt::figure(); - // plt::plot3(initial_x_hcw, initial_y_hcw, initial_z_hcw, "-")->line_width(1).display_name("Initial Trajectory"); - // plt::hold(true); - // plt::plot3(final_x_hcw, final_y_hcw, final_z_hcw, "-")->line_width(1).display_name("Final Trajectory"); - // plt::hold(false); - // plt::xlabel("x_hcw (km)"); - // plt::ylabel("y_hcw (km)"); - // plt::zlabel("z_hcw (km)"); - // plt::legend(); - // plt::title("Initial and Final Trajectories (HCW)"); - // plt::axis(plt::equal); - - // // Plot time history - // plt::figure(); - // plt::plot(initial_t, initial_x_hcw, "-")->line_width(1).display_name("Initial Trajectory"); - // plt::hold(true); - // plt::plot(final_t, final_x_hcw, "-")->line_width(1).display_name("Final Trajectory"); - // plt::hold(false); - // plt::xlabel("Time (s)"); - // plt::show(); - - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - for (int i = 0; i < horizon; ++i) - { - X[i] = initial_state_roe; - U[i] = Eigen::VectorXd::Zero(control_dim); - } - X[horizon] = initial_state_roe; - - // Solver options. - cddp::CDDPOptions options; - options.max_iterations = 1000; - options.tolerance = 1e-6; - options.regularization.initial_value = 1e-5; - options.use_ilqr = true; - options.enable_parallel = true; - options.num_threads = 12; - options.debug = false; - options.ipddp.barrier.mu_initial = 1e-1; - options.msipddp.segment_length = horizon; - options.msipddp.rollout_type = "nonlinear"; - options.ipddp.barrier.mu_update_factor = 0.2; - options.ipddp.barrier.mu_update_power = 1.2; - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state_roe, - goal_state_roe, - horizon, - dt, - std::move(roe_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // goal_state^T * goal_state - std::cout << "initial_state^T * initial_state: " << initial_state_roe.transpose() * initial_state_roe << std::endl; - std::cout << "initial_state: " << initial_state_roe.transpose() << std::endl; - std::cout << "goal_state^T * goal_state: " << goal_state_roe.transpose() * goal_state_roe << std::endl; - std::cout << "goal_state: " << goal_state_roe.transpose() << std::endl; - - // // Add Control Constraint - // Eigen::VectorXd u_upper_accel = Eigen::VectorXd::Constant(control_dim, u_force_max_N); - // cddp_solver.addPathConstraint("ControlConstraint", - // std::make_unique(-u_upper_accel, u_upper_accel)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract the solution - const auto& X_solution_roe = solution.state_trajectory; - const auto& U_solution_accel = solution.control_trajectory; - - // Print the solution - // std::cout << "Solution: " << solution.cost << std::endl; - std::cout << "Solution: " << X_solution_roe.back().transpose() * a_ref_m << std::endl; - std::cout << "Solution: " << U_solution_accel.back().transpose() << std::endl; - - if (!X_solution_roe.empty() && !U_solution_accel.empty()) - { - namespace plt = matplot; - - // Create a new SpacecraftROE instance for transformations, - // as the original roe_system was moved into the solver. - SpacecraftROE roe_transformer_for_plotting(dt, "euler", a_ref_m, nu0_rad); - - // Create time vectors - std::vector t_states(horizon + 1); - for(int i = 0; i <= horizon; ++i) t_states[i] = i * dt; - - std::vector t_controls(horizon); - for(int i = 0; i < horizon; ++i) t_controls[i] = i * dt; - - // Transform ROE states to HCW states (m) for plotting - std::vector x_pos_m(horizon + 1), y_pos_m(horizon + 1), z_pos_m(horizon + 1); - std::vector vx_mps(horizon + 1), vy_mps(horizon + 1), vz_mps(horizon + 1); - - for (size_t i = 0; i < X_solution_roe.size(); ++i) { - const auto& state_roe_current = X_solution_roe[i]; - if (state_roe_current.size() == state_dim) { - double current_time = state_roe_current(6); - Eigen::VectorXd state_hcw_m = roe_transformer_for_plotting.transformROEToHCW(state_roe_current.head(6), current_time); - - x_pos_m[i] = state_hcw_m(0); - y_pos_m[i] = state_hcw_m(1); - z_pos_m[i] = state_hcw_m(2); - vx_mps[i] = state_hcw_m(3); - vy_mps[i] = state_hcw_m(4); - vz_mps[i] = state_hcw_m(5); - } - } - - std::vector ur_accel(horizon), ut_accel(horizon), un_accel(horizon); // Accelerations in km/s^2 - std::vector accel_magnitude_m_s2(horizon); - - for (size_t i = 0; i < U_solution_accel.size(); ++i) { - const auto& control_accel = U_solution_accel[i]; - if (control_accel.size() == control_dim) { - ur_accel[i] = control_accel(0); // m/s^2 - ut_accel[i] = control_accel(1); // m/s^2 - un_accel[i] = control_accel(2); // m/s^2 - accel_magnitude_m_s2[i] = control_accel.norm(); // m/s^2 - } - } - - // --- Generate Plots --- - // plt::figure_size(1200, 800); - - // 1. Position Trajectories (HCW in meters) - plt::figure(); - plt::plot(t_states, x_pos_m)->line_width(2).display_name("x_hcw (m)"); - plt::hold(true); - plt::plot(t_states, y_pos_m)->line_width(2).display_name("y_hcw (m)"); - plt::plot(t_states, z_pos_m)->line_width(2).display_name("z_hcw (m)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("HCW Position vs. Time"); - - // 2. Velocity Trajectories (HCW in m/s) - plt::figure(); - plt::plot(t_states, vx_mps)->line_width(2).display_name("vx_hcw (m/s)"); - plt::hold(true); - plt::plot(t_states, vy_mps)->line_width(2).display_name("vy_hcw (m/s)"); - plt::plot(t_states, vz_mps)->line_width(2).display_name("vz_hcw (m/s)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("HCW Velocity vs. Time"); - - // 5. Control Inputs (Accelerations in km/s^2, ZOH) - plt::figure(); - plt::stairs(t_controls, ur_accel)->line_width(2).display_name("u_r (km/s^2)"); - plt::hold(true); - plt::stairs(t_controls, ut_accel)->line_width(2).display_name("u_t (km/s^2)"); - plt::stairs(t_controls, un_accel)->line_width(2).display_name("u_n (km/s^2)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Acceleration (km/s^2)"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time"); - - // 6. Acceleration Magnitude (km/s^2, ZOH) - plt::figure(); - plt::stairs(t_controls, accel_magnitude_m_s2)->line_width(2).display_name("||Acceleration|| (m/s^2) (ZOH)"); - // plt::hold(true); - // plt::plot(t_controls, std::vector(horizon, u_max_norm_accel), "--r")->display_name("Max Norm Accel"); - // if (u_min_norm_accel > 0) plt::plot(t_controls, std::vector(horizon, u_min_norm_accel), "--y")->display_name("Min Norm Accel"); - // plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Acceleration Magnitude (m/s^2)"); - plt::legend(); - plt::title("Acceleration Magnitude (ZOH) vs. Time"); - - // 7. X-Y plane trajectory (HCW in meters, x-axis vertical, y-axis horizontal to the left) - plt::figure(); - plt::plot(y_pos_m, x_pos_m)->line_width(2).display_name("Trajectory (HCW)"); - plt::hold(true); - if (!x_pos_m.empty() && !y_pos_m.empty()){ - plt::scatter(std::vector{y_pos_m.front()}, std::vector{x_pos_m.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter(std::vector{y_pos_m.back()}, std::vector{x_pos_m.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - // plt::plot(circle_y_m, circle_x_m)->line_width(2).display_name("Ball Constraint (HCW)"); - plt::hold(false); - plt::xlabel("y_hcw (m)"); - plt::ylabel("x_hcw (m)"); - plt::legend(); - plt::title("X-Y Plane Trajectory (HCW)"); - plt::axis(plt::equal); - plt::gca()->x_axis().reverse(true); - - // 8. 3D Trajectory (HCW in meters) - plt::figure(); - plt::plot3(x_pos_m, y_pos_m, z_pos_m, "-o")->line_width(2).marker_size(4).display_name("Trajectory (HCW)"); - plt::hold(true); - if (!x_pos_m.empty()){ - plt::scatter3(std::vector{x_pos_m.front()}, std::vector{y_pos_m.front()}, std::vector{z_pos_m.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter3(std::vector{x_pos_m.back()}, std::vector{y_pos_m.back()}, std::vector{z_pos_m.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - // Could also plot the ball constraint here if it's a sphere. - plt::hold(false); - plt::xlabel("x_hcw (m)"); - plt::ylabel("y_hcw (m)"); - plt::zlabel("z_hcw (m)"); - plt::legend(); - plt::title("3D Trajectory (HCW)"); - plt::axis(plt::equal); - - plt::show(); - std::cout << "Plotting complete." << std::endl; - } - else - { - std::cout << "Solver did not find a solution, or solution variables are not available. Skipping plots." << std::endl; - } - return 0; -} diff --git a/examples/cddp_spacecraft_rpo.cpp b/examples/cddp_spacecraft_rpo.cpp deleted file mode 100644 index c2ab319c..00000000 --- a/examples/cddp_spacecraft_rpo.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -namespace cddp -{ - class SumOfTwoNormObjective : public NonlinearObjective - { - public: - SumOfTwoNormObjective(const Eigen::VectorXd &goal_state, - double weight_running_control, - double weight_terminal_state, - double timestep) - : NonlinearObjective(timestep), - goal_state_(goal_state), - weight_running_control_(weight_running_control), - weight_terminal_state_(weight_terminal_state) - { - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int /*index*/) const override - { - double control_squared = control.squaredNorm(); - double control_norm = std::sqrt(control_squared + epsilon_); - return weight_running_control_ * control_norm; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return weight_terminal_state_ * state_error.squaredNorm(); - } - - // Analytical Gradients - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state.size()); // Or handle error appropriately if size unknown - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - if (norm_u < 1e-8) - { - return Eigen::VectorXd::Zero(control.size()); - } - return weight_running_control_ * control / norm_u; - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return 2.0 * weight_terminal_state_ * state_error; - } - - // Analytical Hessians - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd & state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state.size(), state.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - int control_dim = control.size(); - if (norm_u < 1e-8) - { - return weight_running_control_ * Eigen::MatrixXd::Identity(control_dim, control_dim) / 1e-8; // or a small value like 1.0 - } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd u_ut = control * control.transpose(); - return weight_running_control_ * (I / norm_u - u_ut / (norm_u * norm_u * norm_u)); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd & /*final_state*/) const override - { - int state_dim = goal_state_.size(); - return 2.0 * weight_terminal_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim); - } - - private: - Eigen::VectorXd goal_state_; - double weight_running_control_; - double weight_terminal_state_; - double epsilon_ = 1e-5; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Optimization horizon info - int horizon = 500; // Optimization horizon length - double time_horizon = 10000.0; // Time horizon for optimization [s] - double dt = time_horizon / horizon; // Time step for optimization - int state_dim = 6; - int control_dim = 3; - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - - // Initial state (v-bar hold at 1km) - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, -1000.0, 0.0, 0.0, 0.0, 0.0; - - // Final (reference/goal) state (r-bar ) - Eigen::VectorXd goal_state(state_dim); - goal_state << -100.0, 0.0, 0.0, 0.0, 2*mean_motion*100.0, 0.0; - - // Input constraints - double u_max = 0.05; // for each dimension - double u_min = -0.05; // for each dimension - double u_min_norm = 0.0; // Minimum thrust magnitude - double u_max_norm = 0.05; // Maximum thrust magnitude, consistent with previous u_max - - // Cost weighting for SumOfTwoNormObjective - double weight_running_control = 1.0; // Example value - double weight_terminal_state = 2000.0; // Example value - - // Create the HCW system for optimization - std::unique_ptr hcw_system = - std::make_unique(dt, mean_motion, mass, "euler"); - - // Build cost objective - auto objective = std::make_unique( - goal_state, - weight_running_control, - weight_terminal_state, - dt); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 1000; // May need more iterations for one-shot solve - options.line_search.max_iterations = 21; - options.tolerance = 1e-7; // Tighter tolerance for final solve - options.verbose = true; // Show solver progress - options.enable_parallel = false; - options.num_threads = 8; - // Regularization type is now implicit in new API - options.regularization.initial_value = 1e-3; - options.ipddp.barrier.mu_initial = 1e-0; // Starting barrier coefficient - options.use_ilqr = true; - options.debug = false; - options.msipddp.segment_length = horizon; - options.msipddp.rollout_type = "nonlinear"; - - // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - // Generate initial trajectory by constant initial state - for (int i = 0; i < horizon; ++i) - { - // X[i + 1] = hcw_system->getDiscreteDynamics(X[i], U[i]); - X[i+1] = initial_state; - } - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - dt, - std::move(hcw_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // // Add Thrust Magnitude Constraint - // cddp_solver.addConstraint("MaxThrustMagnitudeConstraint", - // std::make_unique(u_max_norm)); - - // Add Ball Constraint (for keep-out zone) - double radius = 90.0; - Eigen::Vector2d center(0.0, 0.0); - cddp_solver.addPathConstraint("BallConstraint", - std::make_unique(radius, center, 0.1)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract the solution - const auto& X_solution = solution.state_trajectory; - const auto& U_solution = solution.control_trajectory; - - if (!X_solution.empty() && !U_solution.empty()) - { - namespace plt = matplot; - - // Create time vectors - std::vector t_states(horizon + 1); - for(int i = 0; i <= horizon; ++i) t_states[i] = i * dt; - - std::vector t_controls(horizon); - for(int i = 0; i < horizon; ++i) t_controls[i] = i * dt; - - // Extract individual state and control trajectories - std::vector x_pos(horizon + 1), y_pos(horizon + 1), z_pos(horizon + 1); - std::vector vx(horizon + 1), vy(horizon + 1), vz(horizon + 1); - - for (size_t i = 0; i < X_solution.size(); ++i) { - const auto& state = X_solution[i]; - if (state.size() >= state_dim) { // Ensure state has at least 8 dimensions - x_pos[i] = state(0); - y_pos[i] = state(1); - z_pos[i] = state(2); - vx[i] = state(3); - vy[i] = state(4); - vz[i] = state(5); - } - } - - std::vector ux(horizon), uy(horizon), uz(horizon); - std::vector thrust_magnitude(horizon); - - for (size_t i = 0; i < U_solution.size(); ++i) { - const auto& control = U_solution[i]; - if (control.size() >= control_dim) { // Ensure control has at least 3 dimensions - ux[i] = control(0); - uy[i] = control(1); - uz[i] = control(2); - thrust_magnitude[i] = control.norm(); - } - } - - // Circle data for plotting - std::vector circle_x, circle_y; - for (double theta = 0; theta <= 2*M_PI; theta += 0.01) { - circle_x.push_back(radius * std::cos(theta)); - circle_y.push_back(radius * std::sin(theta)); - } - - // --- Generate Plots --- - // plt::figure_size(1200, 800); // Removed due to compilation error - - // 1. Position Trajectories - plt::figure(); - plt::plot(t_states, x_pos)->line_width(2).display_name("x (pos)"); - plt::hold(true); - plt::plot(t_states, y_pos)->line_width(2).display_name("y (pos)"); - plt::plot(t_states, z_pos)->line_width(2).display_name("z (pos)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("Position vs. Time"); - - // 2. Velocity Trajectories - plt::figure(); - plt::plot(t_states, vx)->line_width(2).display_name("vx"); - plt::hold(true); - plt::plot(t_states, vy)->line_width(2).display_name("vy"); - plt::plot(t_states, vz)->line_width(2).display_name("vz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("Velocity vs. Time"); - - // 5. Control Inputs (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, ux)->line_width(2).display_name("ux"); - plt::hold(true); - plt::stairs(t_controls, uy)->line_width(2).display_name("uy"); - plt::stairs(t_controls, uz)->line_width(2).display_name("uz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Input"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time"); - - // 6. Thrust Magnitude (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, thrust_magnitude)->line_width(2).display_name("||Thrust|| (ZOH)"); - // Add norm constraint lines if available - // plt::hold(true); - // plt::plot(t_controls, std::vector(horizon, u_max_norm), "--r")->display_name("Max Norm"); - // if (u_min_norm > 0) plt::plot(t_controls, std::vector(horizon, u_min_norm), "--y")->display_name("Min Norm"); - // plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Thrust Magnitude"); - plt::legend(); - plt::title("Thrust Magnitude (ZOH) vs. Time"); - - // 7. X-Y plane trajectory (x-axis vertical to the top, y-axis horizontal to the left) - plt::figure(); - plt::plot(y_pos, x_pos)->line_width(2).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty() && !y_pos.empty()){ // Check if trajectories are not empty - plt::scatter(std::vector{y_pos.front()}, std::vector{x_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter(std::vector{y_pos.back()}, std::vector{x_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - // Plot Ball Constraint - plt::hold(true); - plt::plot(circle_x, circle_y)->line_width(2).display_name("Ball Constraint"); - plt::hold(false); - plt::xlabel("y (m)"); - plt::ylabel("x (m)"); - plt::legend(); - plt::title("X-Y Plane Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - plt::gca()->x_axis().reverse(true); // Make y-axis (horizontal on plot) increase to the left - - // 8. 3D Trajectory - plt::figure(); - plt::plot3(x_pos, y_pos, z_pos, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty()){ // Check if trajectories are not empty - plt::scatter3(std::vector{x_pos.front()}, std::vector{y_pos.front()}, std::vector{z_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter3(std::vector{x_pos.back()}, std::vector{y_pos.back()}, std::vector{z_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - plt::hold(false); - plt::xlabel("x (m)"); - plt::ylabel("y (m)"); - plt::zlabel("z (m)"); - plt::legend(); - plt::title("3D Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - - // Show all plots - plt::show(); - std::cout << "Plotting complete." << std::endl; - } - else - { - std::cout << "Solver did not find a solution, or solution variables are not available. Skipping plots." << std::endl; - } - return 0; - } \ No newline at end of file diff --git a/examples/cddp_spacecraft_rpo_fuel.cpp b/examples/cddp_spacecraft_rpo_fuel.cpp deleted file mode 100644 index 23fb8ef0..00000000 --- a/examples/cddp_spacecraft_rpo_fuel.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include // For std::sqrt - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "cddp_core/cddp_core.hpp" -#include "cddp_core/objective.hpp" -#include "dynamics_model/spacecraft_linear_fuel.hpp" - -namespace cddp -{ - class MinimizeFuelObjective : public NonlinearObjective - { - public: - MinimizeFuelObjective( - double weight_terminal_fuel, - const Eigen::VectorXd& goal_state_pos_vel, // Should be size 6: [x,y,z,vx,vy,vz] - double weight_terminal_pos, - double weight_terminal_vel, - double timestep) - : NonlinearObjective(timestep), - goal_state_pos_vel_(goal_state_pos_vel), - weight_terminal_fuel_(weight_terminal_fuel), - weight_terminal_pos_(weight_terminal_pos), - weight_terminal_vel_(weight_terminal_vel) - { - } - - double running_cost(const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/, - int /*index*/) const override - { - return 0.0; - } - - double terminal_cost(const Eigen::VectorXd& final_state) const override - { - - double cost = -weight_terminal_fuel_ * final_state(7) * final_state(7); - - // Position cost - for (int i = 0; i < 3; ++i) { - double error = final_state(i) - goal_state_pos_vel_(i); - cost += 0.5 * weight_terminal_pos_ * error * error; - } - - // Velocity cost - for (int i = 0; i < 3; ++i) { - double error = final_state(i + 3) - goal_state_pos_vel_(i + 3); - cost += 0.5 * weight_terminal_vel_ * error * error; - } - return cost; - } - - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd& state, const Eigen::VectorXd& /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state.size()); - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd& /*state*/, const Eigen::VectorXd& control, int /*index*/) const override - { - return Eigen::VectorXd::Zero(control.size()); - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd& final_state) const override - { - Eigen::VectorXd grad = Eigen::VectorXd::Zero(final_state.size()); - grad(7) = weight_terminal_fuel_ * 2.0 * final_state(7); - - // Position gradient - for (int i = 0; i < 3; ++i) { - grad(i) = weight_terminal_pos_ * (final_state(i) - goal_state_pos_vel_(i)); - } - - // Velocity gradient - for (int i = 0; i < 3; ++i) { - grad(i + 3) = weight_terminal_vel_ * (final_state(i + 3) - goal_state_pos_vel_(i + 3)); - } - return grad; - } - - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state.size(), state.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd& /*state*/, const Eigen::VectorXd& control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), control.size()); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd& final_state) const override - { - Eigen::MatrixXd hess = Eigen::MatrixXd::Zero(final_state.size(), final_state.size()); - // Hessian for fuel term is zero - - hess(7, 7) = -weight_terminal_fuel_; - - // Position Hessian - for (int i = 0; i < 3; ++i) { - hess(i, i) = weight_terminal_pos_; - } - - // Velocity Hessian - for (int i = 0; i < 3; ++i) { - hess(i + 3, i + 3) = weight_terminal_vel_; - } - return hess; - } - - private: - double weight_terminal_fuel_; - Eigen::VectorXd goal_state_pos_vel_; // For x, y, z, vx, vy, vz, mass, accumulated_control_effort - double weight_terminal_pos_; - double weight_terminal_vel_; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Optimization horizon info - int horizon = 200; // Optimization horizon length - double time_horizon = 200.0; // Time horizon for optimization [s] - double dt = time_horizon / horizon; // Time step for optimization - int state_dim = 8; - int control_dim = 3; - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - double isp = 300.0; - double g0 = 9.80665; - double nominal_radius = 50.0; - - // Initial state - Eigen::VectorXd initial_state(8); - initial_state << nominal_radius, 0.0, 0.0, 0.0, -2.0*mean_motion*nominal_radius, 0.0, mass, 0.0; - - // Final (reference/goal) state - Eigen::VectorXd goal_state(8); - goal_state << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, mass, 0.0; // Goal is the origin - - // Input constraints - double u_max = 1.0; // for each dimension - double u_min = -1.0; // for each dimension - double u_min_norm = 0.0; // Minimum thrust magnitude - double u_max_norm = 1.0; // Maximum thrust magnitude, consistent with previous u_max - - // Create the HCW system for optimization - std::unique_ptr hcw_system = - std::make_unique(dt, mean_motion, isp, g0, "euler"); - - // Cost weighting - double weight_terminal_fuel = 1.0; - double weight_terminal_pos = 1000.0; - double weight_terminal_vel = 1000.0; - - // Build cost objective - auto objective = std::make_unique( - weight_terminal_fuel, - goal_state, - weight_terminal_pos, - weight_terminal_vel, - dt); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 500; // May need more iterations for one-shot solve - options.line_search.max_iterations = 21; - options.tolerance = 1e-7; // Tighter tolerance for final solve - options.verbose = true; // Show solver progress - options.enable_parallel = false; - options.num_threads = 1; - options.regularization.initial_value = 1e-3; - options.ipddp.barrier.mu_initial = 1e-1; // Starting barrier coefficient - options.use_ilqr = true; - options.debug = true; - options.msipddp.segment_length = horizon / 10; - options.msipddp.rollout_type = "nonlinear"; - - // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - // Generate initial trajectory by constant initial state - for (int i = 0; i < horizon; ++i) - { - X[i + 1] = hcw_system->getDiscreteDynamics(X[i], U[i], i * dt); - // X[i] = initial_state; - } - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - dt, - std::move(hcw_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - // Add Thrust Magnitude Constraint - cddp_solver.addPathConstraint("ThrustMagnitudeConstraint", - std::make_unique(u_min_norm, u_max_norm)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract the solution - const auto& X_solution = solution.state_trajectory; - const auto& U_solution = solution.control_trajectory; - - if (!X_solution.empty() && !U_solution.empty()) - { - namespace plt = matplot; - - // Create time vectors - std::vector t_states(horizon + 1); - for(int i = 0; i <= horizon; ++i) t_states[i] = i * dt; - - std::vector t_controls(horizon); - for(int i = 0; i < horizon; ++i) t_controls[i] = i * dt; - - // Extract individual state and control trajectories - std::vector x_pos(horizon + 1), y_pos(horizon + 1), z_pos(horizon + 1); - std::vector vx(horizon + 1), vy(horizon + 1), vz(horizon + 1); - std::vector mass_traj(horizon + 1), acc_effort_traj(horizon + 1); - - for (size_t i = 0; i < X_solution.size(); ++i) { - const auto& state = X_solution[i]; - if (state.size() >= 8) { // Ensure state has at least 8 dimensions - x_pos[i] = state(0); - y_pos[i] = state(1); - z_pos[i] = state(2); - vx[i] = state(3); - vy[i] = state(4); - vz[i] = state(5); - mass_traj[i] = state(6); - acc_effort_traj[i] = state(7); - } - } - - std::vector ux(horizon), uy(horizon), uz(horizon); - std::vector thrust_magnitude(horizon); - - for (size_t i = 0; i < U_solution.size(); ++i) { - const auto& control = U_solution[i]; - if (control.size() >= 3) { // Ensure control has at least 3 dimensions - ux[i] = control(0); - uy[i] = control(1); - uz[i] = control(2); - thrust_magnitude[i] = control.norm(); - } - } - - // --- Generate Plots --- - // plt::figure_size(1200, 800); // Removed due to compilation error - - // 1. Position Trajectories - plt::figure(); - plt::plot(t_states, x_pos)->line_width(2).display_name("x (pos)"); - plt::hold(true); - plt::plot(t_states, y_pos)->line_width(2).display_name("y (pos)"); - plt::plot(t_states, z_pos)->line_width(2).display_name("z (pos)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("Position vs. Time"); - - // 2. Velocity Trajectories - plt::figure(); - plt::plot(t_states, vx)->line_width(2).display_name("vx"); - plt::hold(true); - plt::plot(t_states, vy)->line_width(2).display_name("vy"); - plt::plot(t_states, vz)->line_width(2).display_name("vz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("Velocity vs. Time"); - - // 3. Mass Trajectory - plt::figure(); - plt::plot(t_states, mass_traj)->line_width(2).display_name("Mass (kg)"); - plt::xlabel("Time (s)"); - plt::ylabel("Mass (kg)"); - plt::legend(); - plt::title("Mass vs. Time"); - - // 4. Accumulated Control Effort - plt::figure(); - plt::plot(t_states, acc_effort_traj)->line_width(2).display_name("Accum. Effort"); - plt::xlabel("Time (s)"); - plt::ylabel("Effort"); - plt::legend(); - plt::title("Accumulated Control Effort vs. Time"); - - // 5. Control Inputs (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, ux)->line_width(2).display_name("ux"); - plt::hold(true); - plt::stairs(t_controls, uy)->line_width(2).display_name("uy"); - plt::stairs(t_controls, uz)->line_width(2).display_name("uz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Input"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time"); - - // 6. Thrust Magnitude (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, thrust_magnitude)->line_width(2).display_name("||Thrust|| (ZOH)"); - // Add norm constraint lines if available - // plt::hold(true); - // plt::plot(t_controls, std::vector(horizon, u_max_norm), "--r")->display_name("Max Norm"); - // if (u_min_norm > 0) plt::plot(t_controls, std::vector(horizon, u_min_norm), "--y")->display_name("Min Norm"); - // plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Thrust Magnitude"); - plt::legend(); - plt::title("Thrust Magnitude (ZOH) vs. Time"); - - // 7. 3D Trajectory - plt::figure(); - plt::plot3(x_pos, y_pos, z_pos, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - plt::hold(true); - // Plot Start and End points - if (!x_pos.empty()){ // Check if trajectories are not empty - plt::scatter3(std::vector{x_pos.front()}, std::vector{y_pos.front()}, std::vector{z_pos.front()}) - ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - plt::scatter3(std::vector{x_pos.back()}, std::vector{y_pos.back()}, std::vector{z_pos.back()}) - ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - } - plt::hold(false); - plt::xlabel("x (m)"); - plt::ylabel("y (m)"); - plt::zlabel("z (m)"); - plt::legend(); - plt::title("3D Trajectory"); - plt::axis(plt::equal); // For aspect_ratio=:equal - - - // Show all plots - plt::show(); - std::cout << "Final mass: " << mass_traj.back() << std::endl; - std::cout << "Plotting complete." << std::endl; - } - else - { - std::cout << "Solver did not find a solution, or solution variables are not available. Skipping plots." << std::endl; - } - - return 0; -} \ No newline at end of file diff --git a/examples/cddp_spacecraft_rpo_mc.cpp b/examples/cddp_spacecraft_rpo_mc.cpp deleted file mode 100644 index b8ef0d98..00000000 --- a/examples/cddp_spacecraft_rpo_mc.cpp +++ /dev/null @@ -1,392 +0,0 @@ -/* - Copyright 2025 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -namespace cddp -{ - class SumOfTwoNormObjective : public NonlinearObjective - { - public: - SumOfTwoNormObjective(const Eigen::VectorXd &goal_state, - double weight_running_control, - double weight_terminal_state, - double timestep) - : NonlinearObjective(timestep), - goal_state_(goal_state), - weight_running_control_(weight_running_control), - weight_terminal_state_(weight_terminal_state) - { - } - - double running_cost(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int /*index*/) const override - { - double control_squared = control.squaredNorm(); - double control_norm = std::sqrt(control_squared + epsilon_); - return weight_running_control_ * control_norm; - } - - double terminal_cost(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return weight_terminal_state_ * state_error.squaredNorm(); - } - - // Analytical Gradients - Eigen::VectorXd getRunningCostStateGradient(const Eigen::VectorXd &state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::VectorXd::Zero(state.size()); // Or handle error appropriately if size unknown - } - - Eigen::VectorXd getRunningCostControlGradient(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - if (norm_u < 1e-8) - { - return Eigen::VectorXd::Zero(control.size()); - } - return weight_running_control_ * control / norm_u; - } - - Eigen::VectorXd getFinalCostGradient(const Eigen::VectorXd &final_state) const override - { - Eigen::VectorXd state_error = final_state - goal_state_; - return 2.0 * weight_terminal_state_ * state_error; - } - - // Analytical Hessians - Eigen::MatrixXd getRunningCostStateHessian(const Eigen::VectorXd &state, const Eigen::VectorXd & /*control*/, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(state.size(), state.size()); - } - - Eigen::MatrixXd getRunningCostControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, int /*index*/) const override - { - double norm_u = std::sqrt(control.squaredNorm() + epsilon_); - int control_dim = control.size(); - if (norm_u < 1e-8) - { - return weight_running_control_ * Eigen::MatrixXd::Identity(control_dim, control_dim) / 1e-8; // or a small value like 1.0 - } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd u_ut = control * control.transpose(); - return weight_running_control_ * (I / norm_u - u_ut / (norm_u * norm_u * norm_u)); - } - - Eigen::MatrixXd getRunningCostCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int /*index*/) const override - { - return Eigen::MatrixXd::Zero(control.size(), state.size()); - } - - Eigen::MatrixXd getFinalCostHessian(const Eigen::VectorXd & /*final_state*/) const override - { - int state_dim = goal_state_.size(); - return 2.0 * weight_terminal_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim); - } - - private: - Eigen::VectorXd goal_state_; - double weight_running_control_; - double weight_terminal_state_; - double epsilon_ = 1e-5; - }; -} // namespace cddp - -namespace fs = std::filesystem; -using namespace cddp; - -int main() -{ - // Monte Carlo parameters - int num_mc_runs = 10; - int successful_runs = 0; - std::vector first_successful_X_solution; - std::vector first_successful_U_solution; - - // Setup random number generation for initial state perturbation - std::mt19937 gen(42); // Fixed seed for reproducibility - // Define distributions for perturbations - - std::normal_distribution<> pos_perturb_dist(0.0, 0.05); - std::normal_distribution<> vel_perturb_dist(0.0, 0.005); - - for (int mc_run = 0; mc_run < num_mc_runs; ++mc_run) - { - std::cout << "\n--- Monte Carlo Run: " << mc_run + 1 << "/" << num_mc_runs << " ---" << std::endl; - - // Optimization horizon info - int horizon = 200; // Optimization horizon length - double time_horizon = 200.0; // Time horizon for optimization [s] - double dt = time_horizon / horizon; // Time step for optimization - int state_dim = 6; - int control_dim = 3; - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - double nominal_radius = 50.0; - - // Initial state (nominal) - Eigen::VectorXd nominal_initial_state(state_dim); - nominal_initial_state << nominal_radius, 0.0, 0.0, 0.0, -2.0 * mean_motion * nominal_radius, 0.0; - - // Perturb initial state - Eigen::VectorXd initial_state = nominal_initial_state; - initial_state(0) += nominal_radius * pos_perturb_dist(gen); // Perturb x - initial_state(1) += nominal_radius * pos_perturb_dist(gen); // Perturb y - initial_state(2) += nominal_radius * pos_perturb_dist(gen); // Perturb z - initial_state(3) += std::abs(nominal_initial_state(4)) * vel_perturb_dist(gen); // Perturb vx (use initial vy magnitude for scaling) - initial_state(4) += std::abs(nominal_initial_state(4)) * vel_perturb_dist(gen); // Perturb vy - initial_state(5) += std::abs(nominal_initial_state(4)) * vel_perturb_dist(gen); // Perturb vz - - // Final (reference/goal) state - Eigen::VectorXd goal_state(state_dim); - goal_state.setZero(); // Goal is the origin - - // Input constraints - double u_max = 1.0; // for each dimension - double u_min = -1.0; // for each dimension - double u_min_norm = 0.0; // Minimum thrust magnitude - double u_max_norm = 1.0; // Maximum thrust magnitude, consistent with previous u_max - - // Cost weighting for SumOfTwoNormObjective - double weight_running_control = 1.0; // Example value - double weight_terminal_state = 1000.0; // Example value - - // Create the HCW system for optimization - std::unique_ptr hcw_system = - std::make_unique(dt, mean_motion, mass, "euler"); - - // Build cost objective - auto objective = std::make_unique( - goal_state, - weight_running_control, - weight_terminal_state, - dt); - - // Setup IPDDP solver options - cddp::CDDPOptions options; - options.max_iterations = 1000; - options.line_search.max_iterations = 21; - options.tolerance = 1e-7; - options.verbose = false; - options.debug = false; - options.print_solver_header = false; - options.enable_parallel = false; - options.num_threads = 1; - // Regularization type is now implicit in new API - options.regularization.initial_value = 1e-5; - options.ipddp.barrier.mu_initial = 1e-1; - options.use_ilqr = true; - options.msipddp.segment_length = horizon / 10; - options.msipddp.rollout_type = "nonlinear"; - - // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - X[0] = initial_state; - for (int i = 0; i < horizon; ++i) - { - X[i + 1] = initial_state; - } - - // Create CDDP solver. - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - dt, - std::move(hcw_system), - std::move(objective), - options); - cddp_solver.setInitialTrajectory(X, U); - - // Add Control Constraint - Eigen::VectorXd u_upper = Eigen::VectorXd::Constant(3, u_max); - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // Solve the Trajectory Optimization Problem - cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); - - const auto& state_trajectory = solution.state_trajectory; - const auto& control_trajectory = solution.control_trajectory; - bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound"); - - if (!state_trajectory.empty() && !control_trajectory.empty() && converged) - { - std::cout << "Run " << mc_run + 1 << " converged." << std::endl; - successful_runs++; - if (first_successful_X_solution.empty()) - { - first_successful_X_solution = state_trajectory; - first_successful_U_solution = control_trajectory; - } - } - else - { - std::cout << "Run " << mc_run + 1 << " did NOT converge or solution is empty." << std::endl; - } - } - - std::cout << "\n--- Monte Carlo Simulation Summary ---" << std::endl; - std::cout << "Total runs: " << num_mc_runs << std::endl; - std::cout << "Successful runs: " << successful_runs << std::endl; - std::cout << "Success rate: " << (static_cast(successful_runs) / num_mc_runs) * 100.0 << "%" << std::endl; - - // Plotting results from the first successful run - if (!first_successful_X_solution.empty() && !first_successful_U_solution.empty()) - { - namespace plt = matplot; - - int horizon = first_successful_U_solution.size(); - double time_horizon = 200.0; // Assuming this is fixed from the problem setup - double dt = time_horizon / horizon; - int state_dim = first_successful_X_solution[0].size(); - int control_dim = first_successful_U_solution[0].size(); - - // Create time vectors - std::vector t_states(horizon + 1); - for (int i = 0; i <= horizon; ++i) - t_states[i] = i * dt; - - std::vector t_controls(horizon); - for (int i = 0; i < horizon; ++i) - t_controls[i] = i * dt; - - // Extract individual state and control trajectories - std::vector x_pos(horizon + 1), y_pos(horizon + 1), z_pos(horizon + 1); - std::vector vx(horizon + 1), vy(horizon + 1), vz(horizon + 1); - - for (size_t i = 0; i < first_successful_X_solution.size(); ++i) - { - const auto &state = first_successful_X_solution[i]; - if (state.size() >= state_dim) - { - x_pos[i] = state(0); - y_pos[i] = state(1); - z_pos[i] = state(2); - vx[i] = state(3); - vy[i] = state(4); - vz[i] = state(5); - } - } - - std::vector ux(horizon), uy(horizon), uz(horizon); - std::vector thrust_magnitude(horizon); - - for (size_t i = 0; i < first_successful_U_solution.size(); ++i) - { - const auto &control = first_successful_U_solution[i]; - if (control.size() >= control_dim) - { - ux[i] = control(0); - uy[i] = control(1); - uz[i] = control(2); - thrust_magnitude[i] = control.norm(); - } - } - - // --- Generate Plots --- - // 1. Position Trajectories - plt::figure(); - plt::plot(t_states, x_pos)->line_width(2).display_name("x (pos)"); - plt::hold(true); - plt::plot(t_states, y_pos)->line_width(2).display_name("y (pos)"); - plt::plot(t_states, z_pos)->line_width(2).display_name("z (pos)"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Position (m)"); - plt::legend(); - plt::title("Position vs. Time (First Successful Run)"); - - // 2. Velocity Trajectories - plt::figure(); - plt::plot(t_states, vx)->line_width(2).display_name("vx"); - plt::hold(true); - plt::plot(t_states, vy)->line_width(2).display_name("vy"); - plt::plot(t_states, vz)->line_width(2).display_name("vz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Velocity (m/s)"); - plt::legend(); - plt::title("Velocity vs. Time (First Successful Run)"); - - // 5. Control Inputs (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, ux)->line_width(2).display_name("ux"); - plt::hold(true); - plt::stairs(t_controls, uy)->line_width(2).display_name("uy"); - plt::stairs(t_controls, uz)->line_width(2).display_name("uz"); - plt::hold(false); - plt::xlabel("Time (s)"); - plt::ylabel("Control Input"); - plt::legend(); - plt::title("Control Inputs (ZOH) vs. Time (First Successful Run)"); - - // 6. Thrust Magnitude (Zeroth-Order Hold) - plt::figure(); - plt::stairs(t_controls, thrust_magnitude)->line_width(2).display_name("||Thrust|| (ZOH)"); - plt::xlabel("Time (s)"); - plt::ylabel("Thrust Magnitude"); - plt::legend(); - plt::title("Thrust Magnitude (ZOH) vs. Time (First Successful Run)"); - - // 7. 3D Trajectory - plt::figure(); - plt::plot3(x_pos, y_pos, z_pos, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - plt::hold(true); - if (!x_pos.empty()) - { - plt::scatter3(std::vector{x_pos.front()}, std::vector{y_pos.front()}, std::vector{z_pos.front()}) - ->marker_color("g") - .marker_style("o") - .marker_size(10) - .display_name("Start"); - plt::scatter3(std::vector{x_pos.back()}, std::vector{y_pos.back()}, std::vector{z_pos.back()}) - ->marker_color("r") - .marker_style("x") - .marker_size(10) - .display_name("End"); - } - plt::hold(false); - plt::xlabel("x (m)"); - plt::ylabel("y (m)"); - plt::zlabel("z (m)"); - plt::legend(); - plt::title("3D Trajectory (First Successful Run)"); - plt::axis(plt::equal); - - // Show all plots - plt::show(); - std::cout << "Plotting complete for the first successful run." << std::endl; - } - else - { - std::cout << "No successful run to plot, or solution variables are not available." << std::endl; - } - return 0; -} diff --git a/examples/cddp_unicycle.cpp b/examples/cddp_unicycle.cpp index df5969a2..7117e598 100644 --- a/examples/cddp_unicycle.cpp +++ b/examples/cddp_unicycle.cpp @@ -13,207 +13,67 @@ See the License for the specific language governing permissions and limitations under the License. */ + #include #include -#include #include "cddp.hpp" #include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; int main() { - // Problem parameters - int state_dim = 3; - int control_dim = 2; - int horizon = 100; - double timestep = 0.03; - std::string integration_type = "euler"; + constexpr double kPi = 3.14159265358979323846; + constexpr int state_dim = 3; + constexpr int control_dim = 2; + constexpr int horizon = 100; + constexpr double timestep = 0.03; - // Create a unicycle instance - std::unique_ptr system = std::make_unique(timestep, integration_type); // Create unique_ptr + Eigen::VectorXd initial_state(state_dim); + initial_state << 0.0, 0.0, kPi / 4.0; - // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 50.0, 0.0, 0.0, - 0.0, 50.0, 0.0, - 0.0, 0.0, 10.0; - Qf = 0.5 * Qf; Eigen::VectorXd goal_state(state_dim); - goal_state << 2.0, 2.0, M_PI/2.0; + goal_state << 2.0, 2.0, kPi / 2.0; - // Create an empty vector of Eigen::VectorXd - std::vector empty_reference_states; - auto objective = std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial and target states - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI/4.0; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); + Qf.diagonal() << 25.0, 25.0, 5.0; - // Set options cddp::CDDPOptions options; - options.max_iterations = 10; + options.max_iterations = 20; options.ipddp.barrier.mu_initial = 1e-2; options.ipddp.barrier.mu_update_factor = 0.1; - // Create CDDP solver with new API - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, - std::move(system), std::move(objective), options); - - // Define constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -1.0, -M_PI; - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.0, M_PI; - - // Add the constraint to the solver - cddp_solver.addPathConstraint(std::string("ControlConstraint"), std::make_unique(control_lower_bound, control_upper_bound)); - auto constraint = cddp_solver.getConstraint("ControlConstraint"); - - // Set initial trajectory - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - cddp_solver.setInitialTrajectory(X, U); - - // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); - - // Extract solution - const auto& X_sol = solution.state_trajectory; // size: horizon + 1 - const auto& U_sol = solution.control_trajectory; // size: horizon - const auto& t_sol = solution.time_points; // size: horizon + 1 - - // Create directory for saving plot (if it doesn't exist) - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Plot the solution (x-y plane) - auto x_arr = cddp::example::extractComponent(X_sol, 0); - auto y_arr = cddp::example::extractComponent(X_sol, 1); - auto theta_arr = cddp::example::extractComponent(X_sol, 2); - - // Plot the solution (control inputs) - auto v_arr = cddp::example::extractComponent(U_sol, 0); - auto omega_arr = cddp::example::extractComponent(U_sol, 1); - - // ----------------------------- - // Plot states and controls - // ----------------------------- - auto f1 = figure(); - f1->size(1200, 800); - - // First subplot: Position Trajectory - auto ax1 = subplot(3, 1, 0); - auto plot_handle = plot(ax1, x_arr, y_arr, "-b"); - plot_handle->line_width(3); - title(ax1, "Position Trajectory"); - xlabel(ax1, "x [m]"); - ylabel(ax1, "y [m]"); - - // Second subplot: Heading Angle vs Time - auto ax2 = subplot(3, 1, 1); - auto heading_plot_handle = plot(ax2, t_sol, theta_arr); - heading_plot_handle->line_width(3); - title(ax2, "Heading Angle"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "theta [rad]"); - - // Fourth subplot: Control Inputs - auto ax4 = subplot(3, 1, 2); - auto p1 = plot(ax4, v_arr, "--b"); - p1->line_width(3); - p1->display_name("Acceleration"); - - hold(ax4, true); - auto p2 = plot(ax4, omega_arr, "--r"); - p2->line_width(3); - p2->display_name("Steering"); - - title(ax4, "Control Inputs"); - xlabel(ax4, "Step"); - ylabel(ax4, "Control"); - matplot::legend(ax4); - - f1->draw(); - f1->save(plotDirectory + "/unicycle_cddp_results.png"); - - // ----------------------------- - // Animation: unicycle Trajectory - // ----------------------------- - auto f2 = figure(); - f2->size(800, 600); - auto ax_anim = f2->current_axes(); - if (!ax_anim) - { - ax_anim = axes(); + cddp::CDDP solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique(timestep, "euler"), + std::make_unique( + Q, R, Qf, goal_state, std::vector{}, timestep), + options); + + Eigen::VectorXd lower(control_dim); + lower << -1.0, -kPi; + Eigen::VectorXd upper(control_dim); + upper << 1.0, kPi; + solver.addPathConstraint("ControlConstraint", + std::make_unique(lower, upper)); + + auto [X, U] = + cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); + solver.setInitialTrajectory(X, U); + + const cddp::CDDPSolution solution = solver.solve(cddp::SolverType::CLDDP); + if (solution.state_trajectory.empty()) { + std::cerr << "Unicycle example failed: empty trajectory" << std::endl; + return 1; } - double car_length = 0.35; - double car_width = 0.15; - - for (size_t i = 0; i < X_sol.size(); ++i) - { - if (i % 10 == 0) - { - ax_anim->clear(); - hold(ax_anim, true); - - double x = x_arr[i]; - double y = y_arr[i]; - double theta = theta_arr[i]; - - // Compute unicycle rectangle corners - std::vector car_x(5), car_y(5); - car_x[0] = x + car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[0] = y + car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[1] = x + car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[1] = y + car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[2] = x - car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[2] = y - car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[3] = x - car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[3] = y - car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - auto car_line = plot(ax_anim, car_x, car_y); - car_line->color("black"); - car_line->line_style("solid"); - car_line->line_width(2); - car_line->display_name("Car"); - - // Plot trajectory up to current frame - std::vector traj_x(x_arr.begin(), x_arr.begin() + i + 1); - std::vector traj_y(y_arr.begin(), y_arr.begin() + i + 1); - auto traj_line = plot(ax_anim, traj_x, traj_y); - traj_line->color("blue"); - traj_line->line_style("solid"); - traj_line->line_width(1.5); - traj_line->display_name("Trajectory"); - - title(ax_anim, "unicycle Trajectory"); - xlabel(ax_anim, "x [m]"); - ylabel(ax_anim, "y [m]"); - xlim(ax_anim, {-1, 2.2}); - ylim(ax_anim, {-1, 2.2}); - // legend(ax_anim); - - std::string filename = plotDirectory + "/unicycle_frame_" + std::to_string(i) + ".png"; - f2->draw(); - f2->save(filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/unicycle_frame_*.png " + plotDirectory + "/unicycle.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/unicycle_frame_*.png"; - std::system(cleanup_command.c_str()); + const Eigen::VectorXd final_error = solution.state_trajectory.back() - goal_state; + std::cout << "Unicycle example completed with status: " + << solution.status_message << '\n' + << "Final objective: " << solution.final_objective << '\n' + << "Final state error norm: " << final_error.norm() << std::endl; + return 0; } diff --git a/examples/cddp_unicycle_mpc.cpp b/examples/cddp_unicycle_mpc.cpp deleted file mode 100644 index e798d161..00000000 --- a/examples/cddp_unicycle_mpc.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -// Helper function to create a circular reference trajectory -std::vector create_circular_trajectory( - double radius, double center_x, double center_y, - double total_time, double dt) -{ - int num_points = static_cast(total_time / dt) + 1; - std::vector trajectory; - trajectory.reserve(num_points); - double angular_velocity = 2.0 * M_PI / total_time; - - for (int i = 0; i < num_points; ++i) - { - double t = i * dt; - double angle = angular_velocity * t; - Eigen::VectorXd state(3); - state(0) = center_x + radius * cos(angle); - state(1) = center_y + radius * sin(angle); - state(2) = angle + M_PI / 2.0; // Tangent to the circle - trajectory.push_back(state); - } - return trajectory; -} - -int main() -{ - // -------------------------- - // 1. Problem and MPC Setup - // -------------------------- - const int state_dim = 3; // [x, y, theta] - const int control_dim = 2; // [v, omega] - const int mpc_horizon = 30; // N in the python notebook - const double mpc_timestep = 0.1; // dt in the python notebook - const std::string integration_type = "euler"; - - // Simulation parameters - const double sim_time = 10.0; - const double sim_dt = 0.1; // controller_dt in python notebook - - // Create a unicycle instance (will be reused) - auto dyn_system_template = std::make_unique(mpc_timestep, integration_type); - - // Quadratic cost matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Q.diagonal() << 30.0, 30.0, 0.1; - Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim); - R.diagonal() << 1.0, 0.1; - Eigen::MatrixXd Qf = 30.0 * Q; // Terminal cost weight from python notebook - - // Create reference trajectory - auto reference_trajectory = create_circular_trajectory(1.0, 0.0, 0.0, sim_time, sim_dt); - - // Initial state - Eigen::VectorXd current_state(state_dim); - current_state << 0.0, -1.0, 0.0; - - // IPDDP Solver Options - cddp::CDDPOptions options_ipddp; - options_ipddp.max_iterations = 15; - options_ipddp.tolerance = 1e-4; - options_ipddp.verbose = false; // Keep it clean for the MPC loop - options_ipddp.debug = false; - options_ipddp.enable_parallel = false; // Can be true for performance - options_ipddp.num_threads = 1; - options_ipddp.regularization.initial_value = 1e-4; - options_ipddp.warm_start = true; - - // Constraint parameters - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.5, M_PI; - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << 0.0, -M_PI; - - // Obstacles (from python notebook) - // We will use the closest one at each step. - std::vector obstacles; - obstacles.push_back(Eigen::Vector3d(0.1, 1.3, 0.6)); - obstacles.push_back(Eigen::Vector3d(-0.85, -0.1, 0.2)); - obstacles.push_back(Eigen::Vector3d(-0.7, -0.75, 0.1)); - - // Simulation history storage - std::vector state_history; - std::vector control_history; - std::vector time_history; - state_history.push_back(current_state); - time_history.push_back(0.0); - - // Initial trajectory guess for the first MPC solve - std::vector X_guess(mpc_horizon + 1, current_state); - std::vector U_guess(mpc_horizon, Eigen::VectorXd::Zero(control_dim)); - - // -------------------------- - // 2. MPC Loop - // -------------------------- - std::cout << "Running IPDDP-based MPC for Unicycle Tracking..." << std::endl; - double current_time = 0.0; - int sim_steps = static_cast(sim_time / sim_dt); - - for (int k = 0; k < sim_steps; ++k) - { - // Get current reference trajectory slice for the MPC horizon - std::vector mpc_ref_traj; - int ref_start_idx = k; - for (int i = 0; i <= mpc_horizon; ++i) - { - int idx = std::min(ref_start_idx + i, (int)reference_trajectory.size() - 1); - mpc_ref_traj.push_back(reference_trajectory[idx]); - } - Eigen::VectorXd mpc_goal_state = mpc_ref_traj.back(); - - // Create objective for this MPC step - auto objective = std::make_unique(Q, R, Qf, mpc_goal_state, mpc_ref_traj, mpc_timestep); - - // Create CDDP solver instance for this MPC step - auto system = std::make_unique(mpc_timestep, integration_type); - cddp::CDDP cddp_solver(current_state, mpc_goal_state, mpc_horizon, mpc_timestep, - std::move(system), std::move(objective), options_ipddp); - - // Find closest obstacle and add constraint - Eigen::Vector2d current_pos = current_state.head(2); - double min_dist = std::numeric_limits::max(); - Eigen::Vector3d closest_obstacle; - for(const auto& obs : obstacles) - { - double dist = (current_pos - obs.head(2)).norm() - obs(2); - if(dist < min_dist) - { - min_dist = dist; - closest_obstacle = obs; - } - } - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - cddp_solver.addPathConstraint("BallConstraint", - std::make_unique(closest_obstacle(2), closest_obstacle.head(2))); - - // Set initial trajectory (warm start) - cddp_solver.setInitialTrajectory(X_guess, U_guess); - - // Solve the OCP - cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); - - // Extract and apply the first control - const auto& status = solution.status_message; - if (status != "OptimalSolutionFound" && status != "AcceptableSolutionFound") - { - std::cerr << "Warning: Solver did not converge at step " << k << ". Status: " << status << std::endl; - // Handle non-convergence, e.g., by applying zero control or previous control - } - - const auto& U_sol = solution.control_trajectory; - Eigen::VectorXd control_to_apply = U_sol[0]; - - // Propagate system dynamics - current_state = dyn_system_template->getDiscreteDynamics(current_state, control_to_apply, 0.0); - - // Update history - state_history.push_back(current_state); - control_history.push_back(control_to_apply); - current_time += sim_dt; - time_history.push_back(current_time); - - // Warm start for the next iteration: shift the solution - const auto& X_sol = solution.state_trajectory; - for(int i = 0; i < mpc_horizon -1; ++i) - { - X_guess[i] = X_sol[i+1]; - U_guess[i] = U_sol[i+1]; - } - X_guess[mpc_horizon -1] = X_sol[mpc_horizon]; - X_guess[mpc_horizon] = X_sol[mpc_horizon]; // Extrapolate or use goal - U_guess[mpc_horizon -1] = U_sol[mpc_horizon-1]; // Hold last control - - std::cout << "MPC Step: " << k+1 << "/" << sim_steps <<", Time: " << current_time << "s, X: [" << current_state.transpose() << "], U: [" << control_to_apply.transpose() << "]" << std::endl; - } - std::cout << "Simulation finished." << std::endl; - - // -------------------------- - // 3. Plotting - // -------------------------- - // Convert trajectories to plottable vectors - std::vector x_hist, y_hist, theta_hist; - for(const auto& s : state_history) - { - x_hist.push_back(s(0)); - y_hist.push_back(s(1)); - theta_hist.push_back(s(2)); - } - std::vector v_hist, omega_hist; - for(const auto& u : control_history) - { - v_hist.push_back(u(0)); - omega_hist.push_back(u(1)); - } - std::vector x_ref, y_ref; - for(const auto& s : reference_trajectory) - { - x_ref.push_back(s(0)); - y_ref.push_back(s(1)); - } - - auto f = figure(true); - f->size(1200, 1000); - f->position(100, 100); - - // Trajectory plot - auto ax1 = subplot(2, 1, 1); - matplot::plot(ax1, x_hist, y_hist, "b-")->line_width(2).display_name("Actual Trajectory"); - matplot::hold(true); - matplot::plot(ax1, x_ref, y_ref, "r--")->line_width(2).display_name("Reference Trajectory"); - - // Plot obstacles - for(const auto& obs : obstacles) - { - std::vector circle_x, circle_y; - for (double theta = 0; theta <= 2*M_PI; theta += 0.01) { - circle_x.push_back(obs(0) + obs(2) * std::cos(theta)); - circle_y.push_back(obs(1) + obs(2) * std::sin(theta)); - } - matplot::plot(ax1, circle_x, circle_y, "k--")->line_width(2).display_name("Obstacle"); - } - - // Mark start and end - auto start_scatter = scatter(ax1, std::vector{x_hist.front()}, std::vector{y_hist.front()}); - start_scatter->marker_color("g").marker_size(100).display_name("Start"); - auto end_scatter = scatter(ax1, std::vector{x_hist.back()}, std::vector{y_hist.back()}); - end_scatter->marker_color("r").marker_size(100).display_name("End"); - - matplot::title(ax1, "Unicycle MPC-CBF Tracking"); - matplot::xlabel(ax1, "X [m]"); - matplot::ylabel(ax1, "Y [m]"); - matplot::legend(ax1, "show"); - matplot::grid(ax1, true); - matplot::axis(ax1, equal); - - // Control plot - auto ax2 = subplot(2, 1, 2); - std::vector control_time_hist = time_history; - control_time_hist.pop_back(); // control history is one step shorter - matplot::plot(ax2, control_time_hist, v_hist, "b-")->line_width(2).display_name("Linear Velocity (v)"); - hold(true); - matplot::plot(ax2, control_time_hist, omega_hist, "g-")->line_width(2).display_name("Angular Velocity (omega)"); - matplot::title(ax2, "Control Inputs vs. Time"); - matplot::xlabel(ax2, "Time [s]"); - matplot::ylabel(ax2, "Control Value"); - matplot::legend(ax2, "show"); - matplot::grid(ax2, true); - - // Save and show plot - const std::string plotDirectory = "../results/examples"; - cddp::example::ensurePlotDir(plotDirectory); - save(plotDirectory + "/unicycle_mpc_cbf.png"); - std::cout << "Saved plot to " << plotDirectory << "/unicycle_mpc_cbf.png" << std::endl; - show(); - - return 0; -} \ No newline at end of file diff --git a/examples/cddp_unicycle_safe.cpp b/examples/cddp_unicycle_safe.cpp deleted file mode 100644 index a8f24bc2..00000000 --- a/examples/cddp_unicycle_safe.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -// Include matplot -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -int main() { - // ------------------------------------------------------- - // 1. Problem Setup - // ------------------------------------------------------- - int state_dim = 3; - int control_dim = 2; - int horizon = 100; - double timestep = 0.03; - std::string integration_type = "euler"; - - // Create a unicycle dynamical system instance - std::unique_ptr dyn_system = - std::make_unique(timestep, integration_type); - - // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.05 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 100.0, 0.0, 0.0, - 0.0, 100.0, 0.0, - 0.0, 0.0, 100.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 2.0, 2.0, M_PI / 2.0; - - // Empty reference states (if needed) - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep - ); - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI / 4.0; - - // CDDP options - cddp::CDDPOptions options; - options.max_iterations = 100; - options.verbose = true; - options.debug = false; - options.tolerance = 1e-5; - options.acceptable_tolerance = 1e-4; - options.regularization.initial_value = 1e-5; - - // Define control box constraints - Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -2.0, -M_PI; - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 2.0, M_PI; - - // ------------------------------------------------------- - // 2. Solve the CDDP Problem (No Ball Constraint) - // ------------------------------------------------------- - cddp::CDDP solver_baseline( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - - // Set constraints - solver_baseline.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - - // Simple initial guess: states = initial_state, controls = zeros - std::vector X_baseline(horizon + 1, initial_state); - std::vector U_baseline(horizon, Eigen::VectorXd::Zero(control_dim)); - solver_baseline.setInitialTrajectory(X_baseline, U_baseline); - - // Solve - cddp::CDDPSolution solution_baseline = solver_baseline.solve(cddp::SolverType::IPDDP); - const auto& X_baseline_sol = solution_baseline.state_trajectory; // size horizon + 1 - - // ------------------------------------------------------- - // 3. Solve with BallConstraint - // ------------------------------------------------------- - cddp::CDDP solver_ball( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - solver_ball.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - - // Add the BallConstraint - double radius = 0.4; - Eigen::Vector2d center(1.0, 1.0); - solver_ball.addPathConstraint("BallConstraint", - std::make_unique(radius, center)); - - // Initial trajectory for the ball-constrained solver - std::vector X_ball(horizon + 1, initial_state); - std::vector U_ball(horizon, Eigen::VectorXd::Zero(control_dim)); - solver_ball.setInitialTrajectory(X_ball, U_ball); - - // Solve - cddp::CDDPSolution solution_ball = solver_ball.solve(cddp::SolverType::IPDDP); - const auto& X_ball_sol = solution_ball.state_trajectory; // horizon+1 - - // ------------------------------------------------------- - // 4. Prepare Data for Plotting - // ------------------------------------------------------- - std::vector x_baseline, y_baseline; - std::vector x_ball_vec, y_ball_vec; - x_baseline.reserve(X_baseline_sol.size()); - y_baseline.reserve(X_baseline_sol.size()); - x_ball_vec.reserve(X_ball_sol.size()); - y_ball_vec.reserve(X_ball_sol.size()); - - // Convert baseline solution states - for (const auto &state : X_baseline_sol) { - x_baseline.push_back(state(0)); - y_baseline.push_back(state(1)); - } - // Convert ball-constrained solution states - for (const auto &state : X_ball_sol) { - x_ball_vec.push_back(state(0)); - y_ball_vec.push_back(state(1)); - } - - // Prepare circle points for the ball constraint - std::vector x_circle, y_circle; - x_circle.reserve(630); // ~ (2*pi / 0.01) - y_circle.reserve(630); - - for (double t = 0.0; t < 2.0 * M_PI; t += 0.01) { - x_circle.push_back(center(0) + radius * std::cos(t)); - y_circle.push_back(center(1) + radius * std::sin(t)); - } - - // ------------------------------------------------------- - // 5. Plot with matplot - // ------------------------------------------------------- - // Create the figure window - auto f1 = figure(true); // 'true' => make it visible (depending on your system) - f1->size(800, 600); - - // Create an axes object - auto ax = f1->current_axes(); - - // Plot the baseline solution (blue) - auto h_baseline = plot(ax, x_baseline, y_baseline); - h_baseline->line_width(2); - h_baseline->display_name("Without Ball Constraint"); - h_baseline->color("blue"); - - // Plot the ball-constrained solution (red) - hold(ax, true); - auto h_ball = plot(ax, x_ball_vec, y_ball_vec); - h_ball->line_width(2); - h_ball->display_name("With Ball Constraint"); - h_ball->color("red"); - - // Plot the constraint circle (green, dashed) - auto h_circle = plot(ax, x_circle, y_circle); - h_circle->line_style("--"); - h_circle->line_width(2); - h_circle->color("green"); - h_circle->display_name("Ball Constraint Region"); - - // Add title, labels, legend - title(ax, "Trajectory Comparison: With vs. Without BallConstraint"); - xlabel(ax, "x"); - ylabel(ax, "y"); - xlim(ax, {-0.5, 2.5}); - ylim(ax, {-0.5, 2.5}); - matplot::legend(ax); - - // Create directory for saving (if not existing) - std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // Save figure - f1->draw(); - f1->save(plotDirectory + "/trajectory_comparison_matplot.png"); - std::cout << "Trajectory comparison saved to " - << plotDirectory + "/trajectory_comparison_matplot.png" << std::endl; - - return 0; -} diff --git a/examples/cddp_unicycle_safe_ipddp.cpp b/examples/cddp_unicycle_safe_ipddp.cpp deleted file mode 100644 index 7afcc951..00000000 --- a/examples/cddp_unicycle_safe_ipddp.cpp +++ /dev/null @@ -1,406 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -int main() { - // -------------------------- - // 1. Problem setup - // -------------------------- - int state_dim = 3; // [x, y, theta] - int control_dim = 2; // [v, omega] - int horizon = 100; - double timestep = 0.03; - std::string integration_type = "euler"; - - // Create a unicycle dynamical system instance - std::unique_ptr dyn_system = - std::make_unique(timestep, integration_type); - - // Objective weighting matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.05 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 100.0, 0.0, 0.0, - 0.0, 100.0, 0.0, - 0.0, 0.0, 100.0; - - // Goal state - Eigen::VectorXd goal_state(state_dim); - goal_state << 2.0, 2.0, M_PI/2.0; - - // Create an empty reference state vector - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI/4.0; - - // CDDP options - cddp::CDDPOptions options; - options.max_iterations = 100; - options.verbose = true; - options.debug = false; - options.enable_parallel = false; - options.num_threads = 1; - options.tolerance = 1e-4; - options.acceptable_tolerance = 1e-3; - options.regularization.initial_value = 1e-2; - options.ipddp.barrier.mu_initial = 1e-1; - - // Define control constraint - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.1, M_PI; // clamp velocity and steering - - // Ball constraint parameters - double radius = 0.4; - Eigen::Vector2d center(1.0, 1.0); - - // Create a directory for saving plots (if it doesn't exist) - const std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - // -------------------------- - // 2. Solve - NO Ball constraint - // -------------------------- - cddp::CDDP solver_baseline( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - // Solver with new API already set up with system and objective - - // Add a control constraint - solver_baseline.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // Naive initial trajectory - std::vector X_baseline_init(horizon + 1, initial_state); - std::vector U_baseline_init(horizon, Eigen::VectorXd::Zero(control_dim)); - solver_baseline.setInitialTrajectory(X_baseline_init, U_baseline_init); - - // Solve - cddp::CDDPSolution solution_baseline = solver_baseline.solve(cddp::SolverType::MSIPDDP); - const auto& X_baseline_sol = solution_baseline.state_trajectory; // horizon+1 - const auto& U_baseline_sol = solution_baseline.control_trajectory; // horizon - const auto& T_baseline_sol = solution_baseline.time_points; // horizon+1 - - // -------------------------- - // 3. Solve - WITH Ball constraint (naive init) - // -------------------------- - cddp::CDDP solver_ball( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - // Solver with new API already set up with system and objective - - // Add constraints - solver_ball.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - solver_ball.addPathConstraint("BallConstraint", - std::make_unique(radius, center)); - - // Naive initial trajectory - std::vector X_ball_init(horizon + 1, initial_state); - std::vector U_ball_init(horizon, Eigen::VectorXd::Zero(control_dim)); - solver_ball.setInitialTrajectory(X_ball_init, U_ball_init); - - // Solve - cddp::CDDPSolution solution_ball = solver_ball.solve(cddp::SolverType::MSIPDDP); - const auto& X_ball_sol = solution_ball.state_trajectory; - const auto& U_ball_sol = solution_ball.control_trajectory; - const auto& T_ball_sol = solution_ball.time_points; - - // -------------------------- - // 4. Solve - WITH Ball constraint (baseline init) - // -------------------------- - cddp::CDDP solver_ball_with_baseline( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - // Solver with new API already set up with system and objective - - // Add constraints - solver_ball_with_baseline.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - solver_ball_with_baseline.addPathConstraint("BallConstraint", - std::make_unique(radius, center)); - - // Use baseline solution as initialization - solver_ball_with_baseline.setInitialTrajectory(X_baseline_sol, U_baseline_sol); - - // Solve - cddp::CDDPSolution solution_ball_with_baseline = solver_ball_with_baseline.solve(cddp::SolverType::IPDDP); - const auto& X_ball_with_baseline_sol = solution_ball_with_baseline.state_trajectory; - const auto& U_ball_with_baseline_sol = solution_ball_with_baseline.control_trajectory; - const auto& T_ball_with_baseline_sol = solution_ball_with_baseline.time_points; - - // -------------------------- - // 5. Convert solutions to std::vectors for plotting - // We'll do: X vs Y, Theta vs time, Controls vs step - // -------------------------- - // Baseline - std::vector x_base, y_base, theta_base, t_base; - std::vector v_base, omega_base; - for (size_t i = 0; i < X_baseline_sol.size(); ++i) { - x_base.push_back(X_baseline_sol[i](0)); - y_base.push_back(X_baseline_sol[i](1)); - theta_base.push_back(X_baseline_sol[i](2)); - if (i < T_baseline_sol.size()) { - t_base.push_back(T_baseline_sol[i]); - } - } - for (size_t i = 0; i < U_baseline_sol.size(); ++i) { - v_base.push_back(U_baseline_sol[i](0)); - omega_base.push_back(U_baseline_sol[i](1)); - } - - // Ball (naive init) - std::vector x_ball, y_ball, theta_ball, t_ball; - std::vector v_ball, omega_ball; - for (size_t i = 0; i < X_ball_sol.size(); ++i) { - x_ball.push_back(X_ball_sol[i](0)); - y_ball.push_back(X_ball_sol[i](1)); - theta_ball.push_back(X_ball_sol[i](2)); - if (i < T_ball_sol.size()) { - t_ball.push_back(T_ball_sol[i]); - } - } - for (size_t i = 0; i < U_ball_sol.size(); ++i) { - v_ball.push_back(U_ball_sol[i](0)); - omega_ball.push_back(U_ball_sol[i](1)); - } - - // Ball with baseline init - std::vector x_ball_bl, y_ball_bl, theta_ball_bl, t_ball_bl; - std::vector v_ball_bl, omega_ball_bl; - for (size_t i = 0; i < X_ball_with_baseline_sol.size(); ++i) { - x_ball_bl.push_back(X_ball_with_baseline_sol[i](0)); - y_ball_bl.push_back(X_ball_with_baseline_sol[i](1)); - theta_ball_bl.push_back(X_ball_with_baseline_sol[i](2)); - if (i < T_ball_with_baseline_sol.size()) { - t_ball_bl.push_back(T_ball_with_baseline_sol[i]); - } - } - for (size_t i = 0; i < U_ball_with_baseline_sol.size(); ++i) { - v_ball_bl.push_back(U_ball_with_baseline_sol[i](0)); - omega_ball_bl.push_back(U_ball_with_baseline_sol[i](1)); - } - - // -------------------------- - // 6. Plot: Subplots for the three solutions - // -------------------------- - auto f1 = figure(true); - f1->size(1200, 800); - - // Subplot 1: XY Trajectories - auto ax1 = subplot(3, 1, 0); - plot(ax1, x_base, y_base, "-b")->display_name("No Ball"); - hold(ax1, true); - plot(ax1, x_ball, y_ball, "-r")->display_name("Ball (naive init)"); - plot(ax1, x_ball_bl, y_ball_bl, "-m")->display_name("Ball (baseline init)"); - - // Also plot the circular region for the ball constraint - std::vector circle_x, circle_y; - for (double th = 0.0; th <= 2.0 * M_PI; th += 0.01) { - circle_x.push_back(center(0) + radius * cos(th)); - circle_y.push_back(center(1) + radius * sin(th)); - } - plot(ax1, circle_x, circle_y, "--g")->display_name("Ball Constraint"); - - title(ax1, "Position Trajectory (X-Y plane)"); - xlabel(ax1, "x [m]"); - ylabel(ax1, "y [m]"); - matplot::legend(ax1); - - // Subplot 2: Heading Angle vs Time - auto ax2 = subplot(3, 1, 1); - plot(ax2, t_base, theta_base, "-b")->display_name("No Ball"); - hold(ax2, true); - plot(ax2, t_ball, theta_ball, "-r")->display_name("Ball (naive init)"); - plot(ax2, t_ball_bl, theta_ball_bl, "-m")->display_name("Ball (baseline init)"); - - title(ax2, "Heading Angle vs Time"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "Theta [rad]"); - matplot::legend(ax2); - - // Subplot 3: Control Inputs vs Step - auto ax3 = subplot(3, 1, 2); - // We'll overlay v and omega in the same subplot for each solution - // "No Ball" in blue - auto p1 = plot(ax3, v_base, "-b"); - p1->display_name("v - No Ball"); - hold(ax3, true); - auto p2 = plot(ax3, omega_base, "--b"); - p2->display_name("omega - No Ball"); - - // "Ball naive" in red - auto p3 = plot(ax3, v_ball, "-r"); - p3->display_name("v - Ball naive"); - auto p4 = plot(ax3, omega_ball, "--r"); - p4->display_name("omega - Ball naive"); - - // "Ball baseline init" in magenta - auto p5 = plot(ax3, v_ball_bl, "-m"); - p5->display_name("v - Ball baseline"); - auto p6 = plot(ax3, omega_ball_bl, "--m"); - p6->display_name("omega - Ball baseline"); - - title(ax3, "Control Inputs vs. Step"); - xlabel(ax3, "Step"); - ylabel(ax3, "Control value"); - matplot::legend(ax3); - - // Save the figure - f1->draw(); - f1->save(plotDirectory + "/trajectory_comparison_ipddp_matplot.png"); - - std::cout << "Saved comparison to " << (plotDirectory + "/trajectory_comparison_ipddp_matplot.png") << std::endl; - - // -------------------------- - // 7. (Optional) Animation for the final solution - // For demonstration, let's animate the "Ball (baseline init)" solution. - // -------------------------- - auto f2 = figure(true); - f2->size(800, 600); - auto ax_anim = f2->current_axes(); - if (!ax_anim) { - ax_anim = axes(); - } - - double unicycle_length = 0.35; - double unicycle_width = 0.15; - - // We'll sample frames every few steps to avoid too many images - for (size_t i = 0; i < X_ball_with_baseline_sol.size(); ++i) { - if (i % 5 == 0) { - ax_anim->clear(); - hold(ax_anim, true); - - double x = x_ball_bl[i]; - double y = y_ball_bl[i]; - double theta = theta_ball_bl[i]; - - // Compute corners of a rectangle representing the unicycle - std::vector car_x(5), car_y(5); - car_x[0] = x + unicycle_length / 2.0 * cos(theta) - - unicycle_width / 2.0 * sin(theta); - car_y[0] = y + unicycle_length / 2.0 * sin(theta) - + unicycle_width / 2.0 * cos(theta); - - car_x[1] = x + unicycle_length / 2.0 * cos(theta) - + unicycle_width / 2.0 * sin(theta); - car_y[1] = y + unicycle_length / 2.0 * sin(theta) - - unicycle_width / 2.0 * cos(theta); - - car_x[2] = x - unicycle_length / 2.0 * cos(theta) - + unicycle_width / 2.0 * sin(theta); - car_y[2] = y - unicycle_length / 2.0 * sin(theta) - - unicycle_width / 2.0 * cos(theta); - - car_x[3] = x - unicycle_length / 2.0 * cos(theta) - - unicycle_width / 2.0 * sin(theta); - car_y[3] = y - unicycle_length / 2.0 * sin(theta) - + unicycle_width / 2.0 * cos(theta); - - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - // Draw the unicycle rectangle - auto car_line = plot(ax_anim, car_x, car_y); - car_line->line_width(2); - - // Plot trajectory up to current frame - std::vector traj_x(x_ball_bl.begin(), x_ball_bl.begin() + i + 1); - std::vector traj_y(y_ball_bl.begin(), y_ball_bl.begin() + i + 1); - auto traj_line = plot(ax_anim, traj_x, traj_y); - traj_line->line_width(1.5); - - // Also show the ball constraint - std::vector circle_x_anim, circle_y_anim; - for (double th = 0.0; th <= 2.0 * M_PI; th += 0.01) { - circle_x_anim.push_back(center(0) + radius * cos(th)); - circle_y_anim.push_back(center(1) + radius * sin(th)); - } - plot(ax_anim, circle_x_anim, circle_y_anim, "--g"); - - title(ax_anim, "Unicycle Trajectory (Ball Constraint, baseline init)"); - xlabel(ax_anim, "x [m]"); - ylabel(ax_anim, "y [m]"); - xlim(ax_anim, {-0.5, 2.5}); - ylim(ax_anim, {-0.5, 2.5}); - - // Save each frame - std::string filename = plotDirectory + "/unicycle_safe_frame_" + std::to_string(i) + ".png"; - f2->draw(); - f2->save(filename); - - // Small pause so we can see the frames - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - } - - // ----------------------------- - // 8. Generate GIF from frames using ImageMagick (if installed) - // ----------------------------- - { - std::string gif_command = - "convert -delay 8 " + plotDirectory + "/unicycle_safe_frame_*.png " + - plotDirectory + "/unicycle_safe.gif"; - std::system(gif_command.c_str()); - - // Cleanup frames if you like - std::string cleanup_command = - "rm " + plotDirectory + "/unicycle_safe_frame_*.png"; - std::system(cleanup_command.c_str()); - } - - return 0; -} diff --git a/examples/cddp_unicycle_safe_ipddp_v2.cpp b/examples/cddp_unicycle_safe_ipddp_v2.cpp deleted file mode 100644 index 240fd970..00000000 --- a/examples/cddp_unicycle_safe_ipddp_v2.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -// Include matplot -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -int main() { - // ------------------------------------------------- - // 1. Problem Setup - // ------------------------------------------------- - int state_dim = 3; - int control_dim = 2; - int horizon = 300; - double timestep = 0.03; - std::string integration_type = "euler"; - - // Create a unicycle dynamical system - auto dyn_system = std::make_unique(timestep, integration_type); - - // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.05 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 100.0, 0.0, 0.0, - 0.0, 100.0, 0.0, - 0.0, 0.0, 100.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 3.0, 3.0, M_PI / 2.0; - - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI / 4.0; - - // CDDP options - cddp::CDDPOptions options; - options.max_iterations = 1000; - options.verbose = true; - options.debug = false; - options.enable_parallel = false; - options.num_threads = 1; - options.tolerance = 1e-5; - options.acceptable_tolerance = 1e-4; - options.regularization.initial_value = 1e-2; - options.ipddp.barrier.mu_initial = 1e-1; - - // Control constraint - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.1, M_PI; - - // ------------------------------------------------- - // 2. Configure and Solve IPDDP with Two BallConstraints - // ------------------------------------------------- - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - - // Solver with new API already set up with system and objective - - // Add constraints - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // First ball constraint - double radius1 = 0.4; - Eigen::Vector2d center1(1.0, 1.0); - cddp_solver.addPathConstraint("BallConstraint", - std::make_unique(radius1, center1)); - - // Second ball constraint - double radius2 = 0.4; - Eigen::Vector2d center2(1.5, 2.5); - cddp_solver.addPathConstraint("BallConstraint2", - std::make_unique(radius2, center2)); - - // Initial trajectory guess - auto [X_sol, U_sol] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); - cddp_solver.setInitialTrajectory(X_sol, U_sol); - - // Solve - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - X_sol = solution.state_trajectory; - U_sol = solution.control_trajectory; - - // ------------------------------------------------- - // 3. Prepare Data for Plotting - // ------------------------------------------------- - std::vector x_sol_plot, y_sol_plot; - x_sol_plot.reserve(X_sol.size()); - y_sol_plot.reserve(X_sol.size()); - - for (const auto &state : X_sol) { - x_sol_plot.push_back(state(0)); - y_sol_plot.push_back(state(1)); - } - - // Create circle points for the two ball constraints - std::vector x_ball1, y_ball1; - std::vector x_ball2, y_ball2; - for (double t = 0.0; t < 2 * M_PI; t += 0.01) { - x_ball1.push_back(center1(0) + radius1 * std::cos(t)); - y_ball1.push_back(center1(1) + radius1 * std::sin(t)); - x_ball2.push_back(center2(0) + radius2 * std::cos(t)); - y_ball2.push_back(center2(1) + radius2 * std::sin(t)); - } - - // ------------------------------------------------- - // 4. Plot with matplot - // ------------------------------------------------- - auto f1 = figure(true); - f1->size(800, 600); - - auto ax = f1->current_axes(); - auto traj_line = plot(ax, x_sol_plot, y_sol_plot); - traj_line->color("blue"); - traj_line->line_width(2); - traj_line->display_name("IPDDP"); - - // We want multiple lines on same plot: - hold(ax, true); - - // Ball constraints (two circles) - auto ball1 = plot(ax, x_ball1, y_ball1); - ball1->line_style("--"); - ball1->line_width(2); - ball1->color("green"); - ball1->display_name("Ball Constraint 1"); - - auto ball2 = plot(ax, x_ball2, y_ball2); - ball2->line_style("--"); - ball2->line_width(2); - ball2->color("green"); - ball2->display_name("Ball Constraint 2"); - - // Turn on grid, label, etc. - grid(ax, true); - xlabel(ax, "x"); - ylabel(ax, "y"); - xlim(ax, {0.0, 4.0}); - ylim(ax, {0.0, 4.0}); - title(ax, "IPDDP Safe Trajectory with Two Ball Constraints"); - matplot::legend(ax); - - // ------------------------------------------------- - // 5. Save Plot - // ------------------------------------------------- - std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - f1->draw(); - f1->save(plotDirectory + "/trajectory_comparison_ipddp_v2_matplot.png"); - std::cout << "Trajectory comparison saved to " - << (plotDirectory + "/trajectory_comparison_ipddp_v2_matplot.png") - << std::endl; - - return 0; -} diff --git a/examples/cddp_unicycle_safe_ipddp_v3.cpp b/examples/cddp_unicycle_safe_ipddp_v3.cpp deleted file mode 100644 index 2eb6f6d4..00000000 --- a/examples/cddp_unicycle_safe_ipddp_v3.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -// Include matplot -#include "matplot/matplot.h" - -namespace fs = std::filesystem; -using namespace matplot; - -int main() { - // ------------------------------------------------- - // 1. Problem Setup - // ------------------------------------------------- - int state_dim = 3; - int control_dim = 2; - int horizon = 300; - double timestep = 0.03; - std::string integration_type = "euler"; - - // Create a unicycle dynamical system - auto dyn_system = std::make_unique(timestep, integration_type); - - // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.05 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 100.0, 0.0, 0.0, - 0.0, 100.0, 0.0, - 0.0, 0.0, 100.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 3.0, 3.0, M_PI / 2.0; - - std::vector empty_reference_states; - auto objective = std::make_unique( - Q, R, Qf, goal_state, empty_reference_states, timestep); - - // Initial state - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI / 4.0; - - // CDDP options - cddp::CDDPOptions options; - options.max_iterations = 1000; - options.verbose = true; - options.debug = false; - options.enable_parallel = false; - options.num_threads = 1; - options.tolerance = 1e-5; - options.acceptable_tolerance = 1e-4; - options.regularization.initial_value = 1e-2; - options.ipddp.barrier.mu_initial = 1e-1; - - // Control constraint - Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.1, M_PI; - - // ------------------------------------------------- - // 2. Configure and Solve IPDDP with Multiple Constraints - // ------------------------------------------------- - cddp::CDDP cddp_solver( - initial_state, - goal_state, - horizon, - timestep, - std::make_unique(timestep, integration_type), - std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), - options - ); - - // Solver with new API already set up with system and objective - - // Add constraints - // Control constraint - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-control_upper_bound, control_upper_bound)); - - // First ball constraint - double radius1 = 0.4; - Eigen::Vector2d center1(1.0, 1.0); - cddp_solver.addPathConstraint("BallConstraint1", - std::make_unique(radius1, center1)); - - // Second ball constraint - double radius2 = 0.4; - Eigen::Vector2d center2(1.5, 2.5); - cddp_solver.addPathConstraint("BallConstraint2", - std::make_unique(radius2, center2)); - - // Linear constraint: y <= 1.0x + 0.5 - Eigen::MatrixXd A(1, state_dim); - A << -1.0, 1.0, 0.0; - Eigen::VectorXd b(1); - b << 0.5; - cddp_solver.addPathConstraint("LinearConstraint", - std::make_unique(A, b)); - - // Initial trajectory guess - auto [X_sol, U_sol] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); - cddp_solver.setInitialTrajectory(X_sol, U_sol); - - // Solve - cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - X_sol = solution.state_trajectory; - U_sol = solution.control_trajectory; - - // Add this line to check the actual starting state in the solution - std::cout << "Actual starting state in solution: " << X_sol[0].transpose() << std::endl; - - // ------------------------------------------------- - // 3. Prepare Data for Plotting - // ------------------------------------------------- - std::vector x_sol_plot, y_sol_plot; - x_sol_plot.reserve(X_sol.size()); - y_sol_plot.reserve(X_sol.size()); - - for (const auto &state : X_sol) { - x_sol_plot.push_back(state(0)); - y_sol_plot.push_back(state(1)); - } - - // Create circle points for the two ball constraints - std::vector x_ball1, y_ball1; - std::vector x_ball2, y_ball2; - for (double t = 0.0; t < 2 * M_PI; t += 0.01) { - x_ball1.push_back(center1(0) + radius1 * std::cos(t)); - y_ball1.push_back(center1(1) + radius1 * std::sin(t)); - x_ball2.push_back(center2(0) + radius2 * std::cos(t)); - y_ball2.push_back(center2(1) + radius2 * std::sin(t)); - } - - // Create points for the linear constraint - std::vector x_linear, y_linear; - for (double x = -1.0; x <= 4.0; x += 0.1) { - double y = 1.0 * x + 0.5; - x_linear.push_back(x); - y_linear.push_back(y); - } - - // ------------------------------------------------- - // 4. Plot with matplot - // ------------------------------------------------- - auto f1 = figure(true); - f1->size(800, 600); - - auto ax = f1->current_axes(); - auto traj_line = plot(ax, x_sol_plot, y_sol_plot); - traj_line->color("blue"); - traj_line->line_width(2); - traj_line->display_name("IPDDP"); - - // We want multiple lines on same plot: - hold(ax, true); - - // Ball constraints (two circles) - auto ball1 = plot(ax, x_ball1, y_ball1); - ball1->line_style("--"); - ball1->line_width(2); - ball1->color("green"); - ball1->display_name("Ball Constraint 1"); - - auto ball2 = plot(ax, x_ball2, y_ball2); - ball2->line_style("--"); - ball2->line_width(2); - ball2->color("green"); - ball2->display_name("Ball Constraint 2"); - - // Linear constraint - auto linear_constraint = plot(ax, x_linear, y_linear); - linear_constraint->line_style("--"); - linear_constraint->line_width(2); - linear_constraint->color("red"); - linear_constraint->display_name("Linear Constraint"); - - // Turn on grid, label, etc. - grid(ax, true); - xlabel(ax, "x"); - ylabel(ax, "y"); - xlim(ax, {-1.0, 4.0}); - ylim(ax, {-1.0, 4.0}); - title(ax, "IPDDP Safe Trajectory with Linear Constraint"); - matplot::legend(ax); - - // ------------------------------------------------- - // 5. Save Plot - // ------------------------------------------------- - std::string plotDirectory = "../results/tests"; - cddp::example::ensurePlotDir(plotDirectory); - - f1->draw(); - f1->save(plotDirectory + "/trajectory_comparison_ipddp_v3_linear_matplot.png"); - std::cout << "Trajectory comparison saved to " - << (plotDirectory + "/trajectory_comparison_ipddp_v3_linear_matplot.png") - << std::endl; - - return 0; -} \ No newline at end of file diff --git a/examples/data/README.md b/examples/data/README.md new file mode 100644 index 00000000..8c7ff56e --- /dev/null +++ b/examples/data/README.md @@ -0,0 +1,9 @@ +# Example Data + +`mpcc_racing_track.csv` is vendored for the Python MPCC portfolio demo. + +Provenance: +- Derived from track data in [`alexliniger/MPCC`](https://github.com/alexliniger/MPCC) +- Method reference: Liniger, A., Domahidi, A., and Morari, M., "Optimization-based autonomous racing of 1:43 scale RC cars," Optimal Control Applications and Methods, 2015. doi: `10.1002/oca.2123` + +The portfolio demo in this repository uses a simplified kinematic contouring-control setup for visualization and Python binding showcase purposes. diff --git a/examples/data/mpcc_racing_track.csv b/examples/data/mpcc_racing_track.csv new file mode 100644 index 00000000..c58f5f3d --- /dev/null +++ b/examples/data/mpcc_racing_track.csv @@ -0,0 +1,490 @@ +x,y,s,heading,curvature,v_ref +-0.836665,1.088823,0.000000,-0.785398,-0.000000,0.000000 +-0.806909,1.059066,0.042081,-0.785398,0.000000,0.000000 +-0.777153,1.029310,0.084163,-0.785398,-0.000000,0.000000 +-0.747397,0.999554,0.126244,-0.785398,0.000000,0.000000 +-0.717641,0.969798,0.168326,-0.785398,-0.000000,0.000000 +-0.687885,0.940042,0.210407,-0.785398,0.000000,0.000000 +-0.658129,0.910286,0.252489,-0.785398,-0.000000,0.000000 +-0.628373,0.880530,0.294570,-0.785398,0.000000,0.000000 +-0.598616,0.850774,0.336652,-0.785398,-0.000000,0.000000 +-0.568860,0.821018,0.378733,-0.785398,-0.000000,0.000000 +-0.539104,0.791262,0.420815,-0.785398,0.000000,0.000000 +-0.509348,0.761505,0.462896,-0.785398,-0.000000,0.000000 +-0.479592,0.731749,0.504978,-0.785398,0.000000,0.000000 +-0.449836,0.701993,0.547059,-0.785398,0.000000,0.000000 +-0.420080,0.672237,0.589141,-0.785398,-0.000000,0.000000 +-0.390324,0.642481,0.631222,-0.785398,-0.000000,0.000000 +-0.360568,0.612725,0.673304,-0.785398,0.000000,0.000000 +-0.330812,0.582969,0.715385,-0.785398,-0.000000,0.000000 +-0.301056,0.553213,0.757467,-0.785398,-0.000000,0.000000 +-0.271299,0.523457,0.799548,-0.785398,0.000000,0.000000 +-0.241543,0.493701,0.841630,-0.785398,0.000000,0.000000 +-0.211787,0.463944,0.883711,-0.785398,-0.000000,0.000000 +-0.182031,0.434188,0.925792,-0.785398,0.000000,0.000000 +-0.152275,0.404432,0.967874,-0.785398,-0.000000,0.000000 +-0.122519,0.374676,1.009955,-0.785398,0.000000,0.000000 +-0.092763,0.344920,1.052037,-0.785398,-0.000000,0.000000 +-0.063007,0.315164,1.094118,-0.785398,-0.000000,0.000000 +-0.033251,0.285408,1.136200,-0.785398,0.000000,0.000000 +-0.003495,0.255652,1.178281,-0.785398,-0.000000,0.000000 +0.026262,0.225896,1.220363,-0.785398,-0.000000,0.000000 +0.056018,0.196140,1.262444,-0.785398,0.000000,0.000000 +0.085774,0.166384,1.304526,-0.785398,-0.000000,0.000000 +0.115530,0.136627,1.346607,-0.785398,0.000000,0.000000 +0.145286,0.106871,1.388689,-0.785398,0.000000,0.000000 +0.175042,0.077115,1.430770,-0.785398,-0.000000,0.000000 +0.204798,0.047359,1.472852,-0.785398,0.000000,0.000000 +0.234554,0.017603,1.514933,-0.785398,0.000000,0.000000 +0.264310,-0.012153,1.557015,-0.785398,-0.000000,0.000000 +0.294066,-0.041909,1.599096,-0.785398,0.000000,0.000000 +0.323823,-0.071665,1.641178,-0.785398,-0.000000,0.000000 +0.353579,-0.101421,1.683259,-0.692998,1.923019,0.000000 +0.381973,-0.124999,1.720166,-0.508199,4.978671,0.000000 +0.414216,-0.142959,1.757074,-0.323399,4.978671,0.000000 +0.449210,-0.154688,1.793981,-0.138600,4.978671,0.000000 +0.485763,-0.159787,1.830888,0.046200,4.978671,0.000000 +0.522631,-0.158082,1.867796,0.230999,4.978671,0.000000 +0.558558,-0.149632,1.904703,0.415799,4.978671,0.000000 +0.592321,-0.134724,1.941610,0.600599,4.978671,0.000000 +0.622769,-0.113867,1.978518,0.785398,4.978671,0.000000 +0.648867,-0.087769,2.015425,0.970198,4.978671,0.000000 +0.669724,-0.057321,2.052333,1.154997,4.978671,0.000000 +0.684632,-0.023558,2.089240,1.339797,4.978671,0.000000 +0.693082,0.012369,2.126147,1.524596,4.978671,0.000000 +0.694787,0.049237,2.163055,1.709396,4.978671,0.000000 +0.689688,0.085790,2.199962,1.894196,4.978671,0.000000 +0.677959,0.120784,2.236869,2.078995,4.978671,0.000000 +0.659999,0.153027,2.273777,2.263795,4.978671,0.000000 +0.636421,0.181421,2.310684,2.356194,3.171086,0.000000 +0.603318,0.214524,2.357498,2.356194,-0.000000,0.000000 +0.570216,0.247627,2.404313,2.356194,0.000000,0.000000 +0.537113,0.280730,2.451128,2.356194,-0.000000,0.000000 +0.504010,0.313833,2.497942,2.356194,0.000000,0.000000 +0.470907,0.346936,2.544757,2.356194,0.000000,0.000000 +0.437804,0.380039,2.591571,2.356194,0.000000,0.000000 +0.404701,0.413142,2.638386,2.356194,-0.000000,0.000000 +0.371598,0.446244,2.685200,2.356194,0.000000,0.000000 +0.338495,0.479347,2.732015,2.356194,-0.000000,0.000000 +0.305393,0.512450,2.778829,2.356194,0.000000,0.000000 +0.272290,0.545553,2.825644,2.356194,-0.000000,0.000000 +0.239187,0.578656,2.872458,2.356194,0.000000,0.000000 +0.206084,0.611759,2.919273,2.282563,-1.209962,0.000000 +0.182539,0.639054,2.955320,2.135301,-4.070573,0.000000 +0.163254,0.669508,2.991366,1.988039,-4.070573,0.000000 +0.148647,0.702462,3.027413,1.840777,-4.070573,0.000000 +0.139033,0.737203,3.063460,1.693515,-4.070573,0.000000 +0.134620,0.772978,3.099506,1.546253,-4.070573,0.000000 +0.135505,0.809014,3.135553,1.398990,-4.070573,0.000000 +0.141667,0.844530,3.171599,1.251728,-4.070573,0.000000 +0.152975,0.878757,3.207646,1.104466,-4.070573,0.000000 +0.169182,0.910955,3.243693,0.957204,-4.070573,0.000000 +0.189937,0.940426,3.279739,0.809942,-4.070573,0.000000 +0.214793,0.966533,3.315786,0.662680,-4.070573,0.000000 +0.243210,0.988710,3.351833,0.515418,-4.070573,0.000000 +0.274574,1.006477,3.387879,0.368155,-4.070573,0.000000 +0.308205,1.019450,3.423926,0.220893,-4.070573,0.000000 +0.343376,1.027348,3.459973,0.073631,-4.070573,0.000000 +0.379325,1.030000,3.496019,0.000000,-2.385515,0.000000 +0.421460,1.030000,3.538154,0.000000,0.000000,0.000000 +0.463595,1.030000,3.580289,0.000000,0.000000,0.000000 +0.505730,1.030000,3.622424,0.000000,0.000000,0.000000 +0.547865,1.030000,3.664559,0.000000,0.000000,0.000000 +0.590000,1.030000,3.706694,-0.031416,-0.650215,0.000000 +0.626732,1.028846,3.743445,-0.094248,-1.708558,0.000000 +0.663320,1.025387,3.780195,-0.157080,-1.708558,0.000000 +0.699618,1.019638,3.816946,-0.219911,-1.708558,0.000000 +0.735484,1.011621,3.853697,-0.282743,-1.708558,0.000000 +0.770775,1.001368,3.890447,-0.345575,-1.708558,0.000000 +0.805353,0.988919,3.927198,-0.408407,-1.708558,0.000000 +0.839081,0.974324,3.963948,-0.471239,-1.708558,0.000000 +0.871826,0.957639,4.000699,-0.534071,-1.708558,0.000000 +0.903459,0.938932,4.037449,-0.596903,-1.708558,0.000000 +0.933854,0.918275,4.074200,-0.659734,-1.708558,0.000000 +0.962893,0.895750,4.110951,-0.722566,-1.708558,0.000000 +0.990460,0.871447,4.147701,-0.785398,-1.708558,0.000000 +1.016447,0.845460,4.184452,-0.848230,-1.708558,0.000000 +1.040750,0.817893,4.221202,-0.911062,-1.708558,0.000000 +1.063275,0.788854,4.257953,-0.973894,-1.708558,0.000000 +1.083932,0.758459,4.294704,-1.036726,-1.708558,0.000000 +1.102639,0.726826,4.331454,-1.099557,-1.708558,0.000000 +1.119324,0.694081,4.368205,-1.162389,-1.708558,0.000000 +1.133919,0.660353,4.404955,-1.225221,-1.708558,0.000000 +1.146368,0.625775,4.441706,-1.288053,-1.708558,0.000000 +1.156621,0.590484,4.478457,-1.350885,-1.708558,0.000000 +1.164638,0.554618,4.515207,-1.413717,-1.708558,0.000000 +1.170387,0.518320,4.551958,-1.476549,-1.708558,0.000000 +1.173846,0.481732,4.588708,-1.539380,-1.708558,0.000000 +1.175000,0.445000,4.625459,-1.570796,-0.852749,0.000000 +1.175000,0.408333,4.662126,-1.570796,0.000000,0.000000 +1.175000,0.371667,4.698792,-1.570796,0.000000,0.000000 +1.175000,0.335000,4.735459,-1.570796,0.000000,0.000000 +1.175000,0.298333,4.772126,-1.570796,0.000000,0.000000 +1.175000,0.261667,4.808792,-1.570796,0.000000,0.000000 +1.175000,0.225000,4.845459,-1.570796,0.000000,0.000000 +1.175000,0.188333,4.882126,-1.570796,0.000000,0.000000 +1.175000,0.151667,4.918792,-1.570796,0.000000,0.000000 +1.175000,0.115000,4.955459,-1.570796,0.000000,0.000000 +1.175000,0.078333,4.992126,-1.570796,0.000000,0.000000 +1.175000,0.041667,5.028792,-1.570796,0.000000,0.000000 +1.175000,0.005000,5.065459,-1.594839,-0.640516,0.000000 +1.174139,-0.030810,5.101279,-1.642925,-1.341894,0.000000 +1.171557,-0.066537,5.137100,-1.806177,-4.554894,0.000000 +1.163171,-0.101505,5.173059,-1.854901,-1.347015,0.000000 +1.153147,-0.135833,5.208820,-1.903632,-1.354607,0.000000 +1.141528,-0.169446,5.244385,-1.952371,-1.362285,0.000000 +1.128358,-0.202269,5.279752,-2.001117,-1.370052,0.000000 +1.113686,-0.234232,5.314921,-2.049870,-1.377908,0.000000 +1.097565,-0.265267,5.349894,-2.098631,-1.385856,0.000000 +1.080051,-0.295309,5.384669,-2.147399,-1.393896,0.000000 +1.061200,-0.324296,5.419246,-2.196176,-1.402030,0.000000 +1.041073,-0.352170,5.453626,-2.244960,-1.410261,0.000000 +1.019735,-0.378875,5.487809,-2.293752,-1.418589,0.000000 +0.997250,-0.404359,5.521795,-2.342553,-1.427017,0.000000 +0.973686,-0.428575,5.555584,-2.391362,-1.435546,0.000000 +0.949113,-0.451478,5.589175,-2.440180,-1.444179,0.000000 +0.923602,-0.473027,5.622569,-2.489006,-1.452917,0.000000 +0.897226,-0.493185,5.655766,-2.537842,-1.461761,0.000000 +0.870061,-0.511920,5.688765,-2.586686,-1.470715,0.000000 +0.842180,-0.529203,5.721568,-2.635539,-1.479780,0.000000 +0.813661,-0.545008,5.754173,-2.684402,-1.488958,0.000000 +0.784582,-0.559313,5.786582,-2.733274,-1.498251,0.000000 +0.755019,-0.572103,5.818793,-2.782156,-1.507662,0.000000 +0.725050,-0.583364,5.850807,-2.831048,-1.517193,0.000000 +0.694755,-0.593087,5.882624,-2.879950,-1.526846,0.000000 +0.664211,-0.601266,5.914244,-2.928862,-1.536623,0.000000 +0.633497,-0.607900,5.945667,-2.977784,-1.546528,0.000000 +0.602689,-0.612992,5.976893,-3.026718,-1.556561,0.000000 +0.571865,-0.616549,6.007922,-3.075662,-1.566727,0.000000 +0.541099,-0.618580,6.038754,-3.124617,-1.577027,0.000000 +0.510469,-0.619100,6.069389,3.109602,-1.587465,0.000000 +0.480046,-0.618127,6.099827,3.060625,-1.598042,0.000000 +0.449904,-0.615681,6.130068,3.011635,-1.608763,0.000000 +0.420113,-0.611787,6.160113,2.962634,-1.619630,0.000000 +0.390742,-0.606474,6.189960,2.913620,-1.630645,0.000000 +0.361858,-0.599773,6.219611,2.864595,-1.641813,0.000000 +0.333527,-0.591718,6.249065,2.815556,-1.653135,0.000000 +0.305811,-0.582348,6.278322,2.766505,-1.664616,0.000000 +0.278771,-0.571701,6.307383,2.717440,-1.676260,0.000000 +0.252465,-0.559822,6.336246,2.668362,-1.688068,0.000000 +0.226948,-0.546757,6.364914,2.619270,-1.700045,0.000000 +0.202274,-0.532553,6.393384,2.570164,-1.712195,0.000000 +0.178492,-0.517262,6.421658,2.521044,-1.724521,0.000000 +0.155650,-0.500935,6.449735,2.471910,-1.737028,0.000000 +0.133790,-0.483629,6.477616,2.422760,-1.749718,0.000000 +0.112956,-0.465398,6.505300,2.373595,-1.762597,0.000000 +0.093184,-0.446303,6.532788,2.324415,-1.775669,0.000000 +0.074509,-0.426401,6.560079,2.428323,3.742864,0.000000 +0.054184,-0.408815,6.586956,2.380237,-1.788392,0.000000 +0.034727,-0.390273,6.613834,2.365055,-0.686297,0.000000 +0.011433,-0.367388,6.646489,2.356194,-0.268906,0.000000 +-0.011452,-0.344503,6.678853,2.356194,-0.000000,0.000000 +-0.034337,-0.321617,6.711217,2.356194,-0.000000,0.000000 +-0.057222,-0.298732,6.743582,2.356194,0.000000,0.000000 +-0.080107,-0.275847,6.775946,2.356194,-0.000000,0.000000 +-0.102992,-0.252962,6.808311,2.356194,-0.000000,0.000000 +-0.125877,-0.230077,6.840675,2.356194,0.000000,0.000000 +-0.148762,-0.207192,6.873039,2.356194,-0.000000,0.000000 +-0.171647,-0.184307,6.905404,2.356194,-0.000000,0.000000 +-0.194533,-0.161422,6.937768,2.356194,0.000000,0.000000 +-0.217418,-0.138537,6.970132,2.356194,0.000000,0.000000 +-0.240303,-0.115652,7.002497,2.356194,-0.000000,0.000000 +-0.263188,-0.092767,7.034861,2.356194,-0.000000,0.000000 +-0.286073,-0.069882,7.067226,2.356194,0.000000,0.000000 +-0.308958,-0.046997,7.099590,2.356194,0.000000,0.000000 +-0.331843,-0.024112,7.131954,2.356194,-0.000000,0.000000 +-0.354728,-0.001227,7.164319,2.356194,0.000000,0.000000 +-0.377613,0.021659,7.196683,2.356194,-0.000000,0.000000 +-0.400498,0.044544,7.229047,2.356194,0.000000,0.000000 +-0.423383,0.067429,7.261412,2.356194,-0.000000,0.000000 +-0.446268,0.090314,7.293776,2.356194,0.000000,0.000000 +-0.469153,0.113199,7.326141,2.356194,-0.000000,0.000000 +-0.492038,0.136084,7.358505,2.356194,0.000000,0.000000 +-0.514923,0.158969,7.390869,2.356194,0.000000,0.000000 +-0.537809,0.181854,7.423234,2.356194,-0.000000,0.000000 +-0.560694,0.204739,7.455598,2.356194,0.000000,0.000000 +-0.583579,0.227624,7.487962,2.446817,3.127606,0.000000 +-0.611387,0.250800,7.524162,2.628063,4.979483,0.000000 +-0.642917,0.268583,7.560361,2.809309,4.979483,0.000000 +-0.677137,0.280391,7.596561,2.990555,4.979483,0.000000 +-0.712924,0.285838,7.632761,-3.111385,4.979483,0.000000 +-0.749107,0.284745,7.668960,-2.930139,4.979483,0.000000 +-0.784501,0.277147,7.705160,-2.748894,4.979483,0.000000 +-0.817945,0.263294,7.741359,-2.567648,4.979483,0.000000 +-0.848344,0.243639,7.777559,-2.386402,4.979483,0.000000 +-0.874702,0.218827,7.813758,-2.205156,4.979483,0.000000 +-0.896156,0.189670,7.849958,-2.023911,4.979483,0.000000 +-0.912003,0.157124,7.886157,-1.842665,4.979483,0.000000 +-0.921724,0.122254,7.922357,-1.661419,4.979483,0.000000 +-0.925000,0.086203,7.958557,-1.570796,2.523604,0.000000 +-0.925000,0.049661,7.995098,-1.570796,0.000000,0.000000 +-0.925000,0.013120,8.031639,-1.570796,0.000000,0.000000 +-0.925000,-0.023421,8.068181,-1.570796,0.000000,0.000000 +-0.925000,-0.059963,8.104722,-1.570796,0.000000,0.000000 +-0.925000,-0.096504,8.141263,-1.570796,0.000000,0.000000 +-0.925000,-0.133045,8.177805,-1.570796,0.000000,0.000000 +-0.925000,-0.169587,8.214346,-1.570796,0.000000,0.000000 +-0.925000,-0.206128,8.250887,-1.570796,0.000000,0.000000 +-0.925000,-0.242669,8.287429,-1.570796,0.000000,0.000000 +-0.925000,-0.279211,8.323970,-1.570796,0.000000,0.000000 +-0.925000,-0.315752,8.360511,-1.570796,0.000000,0.000000 +-0.925000,-0.352293,8.397053,-1.570796,0.000000,0.000000 +-0.925000,-0.388835,8.433594,-1.570796,0.000000,0.000000 +-0.925000,-0.425376,8.470135,-1.570796,0.000000,0.000000 +-0.925000,-0.461917,8.506677,-1.570796,0.000000,0.000000 +-0.925000,-0.498459,8.543218,-1.570796,0.000000,0.000000 +-0.925000,-0.535000,8.579759,-1.505346,1.826009,0.000000 +-0.922562,-0.572200,8.617039,-1.374447,3.501259,0.000000 +-0.915289,-0.608763,8.654319,-1.243547,3.501259,0.000000 +-0.903306,-0.644065,8.691599,-1.112647,3.501259,0.000000 +-0.886817,-0.677500,8.728878,-0.981748,3.501259,0.000000 +-0.866106,-0.708497,8.766158,-0.850848,3.501259,0.000000 +-0.841525,-0.736525,8.803438,-0.719948,3.501259,0.000000 +-0.813497,-0.761106,8.840718,-0.589049,3.501259,0.000000 +-0.782500,-0.781817,8.877998,-0.458149,3.501259,0.000000 +-0.749065,-0.798306,8.915277,-0.327249,3.501259,0.000000 +-0.713763,-0.810289,8.952557,-0.196350,3.501259,0.000000 +-0.677200,-0.817562,8.989837,-0.065450,3.501259,0.000000 +-0.640000,-0.820000,9.027117,0.000000,1.529449,0.000000 +-0.607500,-0.820000,9.059617,0.000000,0.000000,0.000000 +-0.575000,-0.820000,9.092117,-0.092400,-3.224028,0.000000 +-0.538250,-0.823405,9.129024,-0.277199,-4.978671,0.000000 +-0.502752,-0.833506,9.165931,-0.461999,-4.978671,0.000000 +-0.469714,-0.849957,9.202839,-0.646798,-4.978671,0.000000 +-0.440261,-0.872198,9.239746,-0.831598,-4.978671,0.000000 +-0.415397,-0.899473,9.276653,-1.016398,-4.978671,0.000000 +-0.395967,-0.930852,9.313561,-1.201197,-4.978671,0.000000 +-0.382635,-0.965267,9.350468,-1.385997,-4.978671,0.000000 +-0.375853,-1.001546,9.387375,-1.570796,-4.978671,0.000000 +-0.375853,-1.038454,9.424283,-1.755596,-4.978671,0.000000 +-0.382635,-1.074733,9.461190,-1.940395,-4.978671,0.000000 +-0.395967,-1.109148,9.498097,-2.125195,-4.978671,0.000000 +-0.415397,-1.140527,9.535005,-2.309995,-4.978671,0.000000 +-0.440261,-1.167802,9.571912,-2.494794,-4.978671,0.000000 +-0.469714,-1.190043,9.608819,-2.679594,-4.978671,0.000000 +-0.502752,-1.206494,9.645727,-2.864393,-4.978671,0.000000 +-0.538250,-1.216595,9.682634,-3.049193,-4.978671,0.000000 +-0.575000,-1.220000,9.719542,3.141593,-2.540145,0.000000 +-0.612500,-1.220000,9.757042,3.141593,0.000000,0.000000 +-0.650000,-1.220000,9.794542,3.141593,0.000000,0.000000 +-0.687500,-1.220000,9.832042,3.141593,0.000000,0.000000 +-0.725000,-1.220000,9.869542,-3.049193,2.421604,0.000000 +-0.761750,-1.223405,9.906449,-2.864393,4.978671,0.000000 +-0.797248,-1.233506,9.943356,-2.679594,4.978671,0.000000 +-0.830286,-1.249957,9.980264,-2.494794,4.978671,0.000000 +-0.859739,-1.272198,10.017171,-2.309995,4.978671,0.000000 +-0.884603,-1.299473,10.054078,-2.125195,4.978671,0.000000 +-0.904033,-1.330852,10.090986,-1.940395,4.978671,0.000000 +-0.917365,-1.365267,10.127893,-1.755596,4.978671,0.000000 +-0.924147,-1.401546,10.164800,-1.570796,4.978671,0.000000 +-0.924147,-1.438454,10.201708,-1.385997,4.978671,0.000000 +-0.917365,-1.474733,10.238615,-1.201197,4.978671,0.000000 +-0.904033,-1.509148,10.275522,-1.016398,4.978671,0.000000 +-0.884603,-1.540527,10.312430,-0.831598,4.978671,0.000000 +-0.859739,-1.567802,10.349337,-0.646798,4.978671,0.000000 +-0.830286,-1.590043,10.386244,-0.461999,4.978671,0.000000 +-0.797248,-1.606494,10.423152,-0.277199,4.978671,0.000000 +-0.761750,-1.616595,10.460059,-0.092400,4.978671,0.000000 +-0.725000,-1.620000,10.496966,0.000000,2.488606,0.000000 +-0.688261,-1.620000,10.533706,0.000000,0.000000,0.000000 +-0.651522,-1.620000,10.570445,0.000000,0.000000,0.000000 +-0.614783,-1.620000,10.607184,0.000000,0.000000,0.000000 +-0.578043,-1.620000,10.643923,0.000000,0.000000,0.000000 +-0.541304,-1.620000,10.680662,0.000000,0.000000,0.000000 +-0.504565,-1.620000,10.717401,0.000000,0.000000,0.000000 +-0.467826,-1.620000,10.754140,0.000000,0.000000,0.000000 +-0.431087,-1.620000,10.790879,0.000000,0.000000,0.000000 +-0.394348,-1.620000,10.827619,0.000000,0.000000,0.000000 +-0.357609,-1.620000,10.864358,0.000000,0.000000,0.000000 +-0.320870,-1.620000,10.901097,0.000000,0.000000,0.000000 +-0.284130,-1.620000,10.937836,0.000000,0.000000,0.000000 +-0.247391,-1.620000,10.974575,0.000000,0.000000,0.000000 +-0.210652,-1.620000,11.011314,0.000000,0.000000,0.000000 +-0.173913,-1.620000,11.048053,0.000000,0.000000,0.000000 +-0.137174,-1.620000,11.084792,0.000000,0.000000,0.000000 +-0.100435,-1.620000,11.121532,0.000000,0.000000,0.000000 +-0.063696,-1.620000,11.158271,0.000000,0.000000,0.000000 +-0.026957,-1.620000,11.195010,0.000000,0.000000,0.000000 +0.009783,-1.620000,11.231749,0.000000,0.000000,0.000000 +0.046522,-1.620000,11.268488,0.000000,0.000000,0.000000 +0.083261,-1.620000,11.305227,0.000000,0.000000,0.000000 +0.119500,-1.620000,11.341466,0.098175,2.714079,0.000000 +0.155689,-1.616436,11.377831,0.294524,5.364877,0.000000 +0.190488,-1.605880,11.414195,0.490874,5.364877,0.000000 +0.222558,-1.588738,11.450559,0.687223,5.364877,0.000000 +0.250668,-1.565668,11.486924,0.883573,5.364877,0.000000 +0.273738,-1.537558,11.523288,1.079922,5.364877,0.000000 +0.290880,-1.505488,11.559653,1.276272,5.364877,0.000000 +0.301436,-1.470689,11.596017,1.472622,5.364877,0.000000 +0.305000,-1.434500,11.632381,1.469850,-0.074123,0.000000 +0.308564,-1.399311,11.667751,1.276272,-5.591955,0.000000 +0.319120,-1.364512,11.704115,1.079922,-5.364877,0.000000 +0.336262,-1.332442,11.740479,0.883573,-5.364877,0.000000 +0.359332,-1.304332,11.776844,0.687223,-5.364877,0.000000 +0.387442,-1.281262,11.813208,0.490874,-5.364877,0.000000 +0.419512,-1.264120,11.849572,0.294524,-5.364877,0.000000 +0.454311,-1.253564,11.885937,0.098175,-5.364877,0.000000 +0.490500,-1.250000,11.922301,0.000000,-2.631349,0.000000 +0.526000,-1.250000,11.957801,0.000000,0.000000,0.000000 +0.562000,-1.250000,11.993801,0.000000,0.000000,0.000000 +0.598000,-1.250000,12.029801,0.000000,0.000000,0.000000 +0.634000,-1.250000,12.065801,0.000000,0.000000,0.000000 +0.670000,-1.250000,12.101801,0.000000,0.000000,0.000000 +0.706000,-1.250000,12.137801,0.000000,0.000000,0.000000 +0.742000,-1.250000,12.173801,0.000000,0.000000,0.000000 +0.778000,-1.250000,12.209801,0.000000,0.000000,0.000000 +0.814000,-1.250000,12.245801,0.000000,0.000000,0.000000 +0.850000,-1.250000,12.281801,0.023800,0.659845,0.000000 +0.885924,-1.249145,12.317736,0.071400,1.324128,0.000000 +0.921767,-1.246581,12.353670,0.119000,1.324128,0.000000 +0.957448,-1.242315,12.389605,0.166600,1.324128,0.000000 +0.992885,-1.236356,12.425539,0.214199,1.324128,0.000000 +1.027998,-1.228718,12.461474,0.261799,1.324128,0.000000 +1.062708,-1.219417,12.497408,0.309399,1.324128,0.000000 +1.096936,-1.208476,12.533343,0.356999,1.324128,0.000000 +1.130605,-1.195918,12.569277,0.404599,1.324128,0.000000 +1.163638,-1.181772,12.605212,0.452199,1.324128,0.000000 +1.195961,-1.166071,12.641146,0.499799,1.324128,0.000000 +1.227500,-1.148849,12.677081,0.547399,1.324128,0.000000 +1.258184,-1.130146,12.713015,0.594999,1.324128,0.000000 +1.287943,-1.110005,12.748950,0.642598,1.324128,0.000000 +1.316710,-1.088470,12.784884,0.690198,1.324128,0.000000 +1.344420,-1.065591,12.820819,0.737798,1.324128,0.000000 +1.371010,-1.041419,12.856753,0.785398,1.324128,0.000000 +1.396419,-1.016010,12.892688,0.832998,1.324128,0.000000 +1.420591,-0.989420,12.928622,0.880598,1.324128,0.000000 +1.443470,-0.961710,12.964557,0.928198,1.324128,0.000000 +1.465005,-0.932943,13.000492,0.975798,1.324128,0.000000 +1.485146,-0.903184,13.036426,1.023398,1.324128,0.000000 +1.503849,-0.872500,13.072361,1.070997,1.324128,0.000000 +1.521071,-0.840961,13.108295,1.118597,1.324128,0.000000 +1.536772,-0.808638,13.144230,1.166197,1.324128,0.000000 +1.550918,-0.775605,13.180164,1.213797,1.324128,0.000000 +1.563476,-0.741936,13.216099,1.261397,1.324128,0.000000 +1.574417,-0.707708,13.252033,1.308997,1.324128,0.000000 +1.583718,-0.672998,13.287968,1.356597,1.324128,0.000000 +1.591356,-0.637885,13.323902,1.404197,1.324128,0.000000 +1.597315,-0.602448,13.359837,1.451797,1.324128,0.000000 +1.601581,-0.566767,13.395771,1.499396,1.324128,0.000000 +1.604145,-0.530924,13.431706,1.546996,1.324128,0.000000 +1.605000,-0.495000,13.467640,1.570796,0.662093,0.000000 +1.605000,-0.459074,13.503566,1.570796,0.000000,0.000000 +1.605000,-0.423148,13.539492,1.570796,0.000000,0.000000 +1.605000,-0.387222,13.575418,1.570796,0.000000,0.000000 +1.605000,-0.351296,13.611344,1.570796,0.000000,0.000000 +1.605000,-0.315370,13.647270,1.570796,0.000000,0.000000 +1.605000,-0.279444,13.683196,1.570796,0.000000,0.000000 +1.605000,-0.243519,13.719122,1.570796,0.000000,0.000000 +1.605000,-0.207593,13.755048,1.570796,0.000000,0.000000 +1.605000,-0.171667,13.790974,1.570796,0.000000,0.000000 +1.605000,-0.135741,13.826900,1.570796,0.000000,0.000000 +1.605000,-0.099815,13.862826,1.570796,0.000000,0.000000 +1.605000,-0.063889,13.898751,1.570796,0.000000,0.000000 +1.605000,-0.027963,13.934677,1.570796,0.000000,0.000000 +1.605000,0.007963,13.970603,1.570796,0.000000,0.000000 +1.605000,0.043889,14.006529,1.570796,0.000000,0.000000 +1.605000,0.079815,14.042455,1.570796,0.000000,0.000000 +1.605000,0.115741,14.078381,1.570796,0.000000,0.000000 +1.605000,0.151667,14.114307,1.570796,0.000000,0.000000 +1.605000,0.187593,14.150233,1.570796,0.000000,0.000000 +1.605000,0.223519,14.186159,1.570796,0.000000,0.000000 +1.605000,0.259444,14.222085,1.570796,0.000000,0.000000 +1.605000,0.295370,14.258011,1.570796,0.000000,0.000000 +1.605000,0.331296,14.293937,1.570796,0.000000,0.000000 +1.605000,0.367222,14.329863,1.570796,0.000000,0.000000 +1.605000,0.403148,14.365788,1.570796,0.000000,0.000000 +1.605000,0.439074,14.401714,1.570796,0.000000,0.000000 +1.605000,0.475000,14.437640,1.589061,0.509149,0.000000 +1.604343,0.510974,14.473621,1.625592,1.015059,0.000000 +1.602372,0.546900,14.509601,1.662122,1.015059,0.000000 +1.599091,0.582731,14.545581,1.698652,1.015059,0.000000 +1.594503,0.618417,14.581561,1.735182,1.015059,0.000000 +1.588615,0.653912,14.617541,1.771712,1.015059,0.000000 +1.581435,0.689169,14.653522,1.808242,1.015059,0.000000 +1.572971,0.724139,14.689502,1.844772,1.015059,0.000000 +1.563237,0.758778,14.725482,1.881303,1.015059,0.000000 +1.552243,0.793037,14.761462,1.917833,1.015059,0.000000 +1.540006,0.826872,14.797442,1.954363,1.015059,0.000000 +1.526541,0.860238,14.833422,1.990893,1.015059,0.000000 +1.511867,0.893090,14.869403,2.027423,1.015059,0.000000 +1.496002,0.925384,14.905383,2.063953,1.015059,0.000000 +1.478969,0.957077,14.941363,2.100483,1.015059,0.000000 +1.460789,0.988126,14.977343,2.137014,1.015059,0.000000 +1.441488,1.018491,15.013323,2.173544,1.015059,0.000000 +1.421090,1.048131,15.049304,2.210074,1.015059,0.000000 +1.399624,1.077006,15.085284,2.246604,1.015059,0.000000 +1.377117,1.105078,15.121264,2.283134,1.015059,0.000000 +1.353601,1.132309,15.157244,2.319664,1.015059,0.000000 +1.329105,1.158663,15.193224,2.356194,1.015059,0.000000 +1.303663,1.184105,15.229205,2.392725,1.015059,0.000000 +1.277309,1.208601,15.265185,2.429255,1.015059,0.000000 +1.250078,1.232117,15.301165,2.465785,1.015059,0.000000 +1.222006,1.254624,15.337145,2.502315,1.015059,0.000000 +1.193131,1.276090,15.373125,2.538845,1.015059,0.000000 +1.163491,1.296488,15.409106,2.575375,1.015059,0.000000 +1.133126,1.315789,15.445086,2.611906,1.015059,0.000000 +1.102077,1.333969,15.481066,2.648436,1.015059,0.000000 +1.070384,1.351002,15.517046,2.684966,1.015059,0.000000 +1.038090,1.366867,15.553026,2.721496,1.015059,0.000000 +1.005238,1.381541,15.589007,2.758026,1.015059,0.000000 +0.971872,1.395006,15.624987,2.794556,1.015059,0.000000 +0.938037,1.407243,15.660967,2.831086,1.015059,0.000000 +0.903778,1.418237,15.696947,2.867617,1.015059,0.000000 +0.869139,1.427971,15.732927,2.904147,1.015059,0.000000 +0.834169,1.436435,15.768908,2.940677,1.015059,0.000000 +0.798912,1.443615,15.804888,2.977207,1.015059,0.000000 +0.763417,1.449503,15.840868,3.013737,1.015059,0.000000 +0.727731,1.454091,15.876848,3.050267,1.015059,0.000000 +0.691900,1.457372,15.912828,3.086797,1.015059,0.000000 +0.655974,1.459343,15.948808,3.123328,1.015059,0.000000 +0.620000,1.460000,15.984789,3.141593,0.512851,0.000000 +0.583649,1.460000,16.021140,3.141593,0.000000,0.000000 +0.547297,1.460000,16.057491,3.141593,0.000000,0.000000 +0.510946,1.460000,16.093843,3.141593,0.000000,0.000000 +0.474595,1.460000,16.130194,3.141593,0.000000,0.000000 +0.438243,1.460000,16.166545,3.141593,0.000000,0.000000 +0.401892,1.460000,16.202897,3.141593,0.000000,0.000000 +0.365541,1.460000,16.239248,3.141593,0.000000,0.000000 +0.329189,1.460000,16.275600,3.141593,0.000000,0.000000 +0.292838,1.460000,16.311951,3.141593,0.000000,0.000000 +0.256486,1.460000,16.348302,3.141593,0.000000,0.000000 +0.220135,1.460000,16.384654,3.141593,0.000000,0.000000 +0.183784,1.460000,16.421005,3.141593,0.000000,0.000000 +0.147432,1.460000,16.457356,3.141593,0.000000,0.000000 +0.111081,1.460000,16.493708,3.141593,0.000000,0.000000 +0.074730,1.460000,16.530059,3.141593,0.000000,0.000000 +0.038378,1.460000,16.566410,3.141593,0.000000,0.000000 +0.002027,1.460000,16.602762,3.141593,0.000000,0.000000 +-0.034324,1.460000,16.639113,3.141593,0.000000,0.000000 +-0.070676,1.460000,16.675464,3.141593,0.000000,0.000000 +-0.107027,1.460000,16.711816,3.141593,0.000000,0.000000 +-0.143378,1.460000,16.748167,3.141593,0.000000,0.000000 +-0.179730,1.460000,16.784518,3.141593,0.000000,0.000000 +-0.216081,1.460000,16.820870,3.141593,0.000000,0.000000 +-0.252432,1.460000,16.857221,3.141593,0.000000,0.000000 +-0.288784,1.460000,16.893572,3.141593,0.000000,0.000000 +-0.325135,1.460000,16.929924,3.141593,0.000000,0.000000 +-0.361486,1.460000,16.966275,3.141593,0.000000,0.000000 +-0.397838,1.460000,17.002627,3.141593,0.000000,0.000000 +-0.434189,1.460000,17.038978,3.141593,0.000000,0.000000 +-0.470541,1.460000,17.075329,3.141593,0.000000,0.000000 +-0.506892,1.460000,17.111681,3.141593,0.000000,0.000000 +-0.543243,1.460000,17.148032,3.141593,0.000000,0.000000 +-0.579595,1.460000,17.184383,3.141593,0.000000,0.000000 +-0.615946,1.460000,17.220735,3.141593,0.000000,0.000000 +-0.652297,1.460000,17.257086,3.141593,0.000000,0.000000 +-0.688649,1.460000,17.293437,3.141593,0.000000,0.000000 +-0.725000,1.460000,17.329789,-3.050970,2.479164,0.000000 +-0.761051,1.456724,17.365988,-2.869724,4.979483,0.000000 +-0.795921,1.447003,17.402188,-2.688478,4.979483,0.000000 +-0.828468,1.431156,17.438387,-2.507233,4.979483,0.000000 +-0.857625,1.409702,17.474587,-2.325987,4.979483,0.000000 +-0.882437,1.383344,17.510786,-2.144741,4.979483,0.000000 +-0.902091,1.352945,17.546986,-1.963495,4.979483,0.000000 +-0.915944,1.319501,17.583186,-1.782250,4.979483,0.000000 +-0.923542,1.284107,17.619385,-1.601004,4.979483,0.000000 +-0.924635,1.247924,17.655585,-1.419758,4.979483,0.000000 +-0.919188,1.212137,17.691784,-1.238512,4.979483,0.000000 +-0.907380,1.177917,17.727984,-1.057267,4.979483,0.000000 +-0.889597,1.146387,17.764183,-0.876021,4.979483,0.000000 +-0.866421,1.118579,17.800383,-0.785398,2.906215,0.000000 diff --git a/examples/hessian_example.cpp b/examples/hessian_example.cpp deleted file mode 100644 index 107b9c46..00000000 --- a/examples/hessian_example.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include - -#include "dynamics_model/pendulum.hpp" -#include "dynamics_model/dubins_car.hpp" - -using namespace cddp; - -// Helper function to print a matrix with a label -void printMatrix(const std::string& label, const Eigen::MatrixXd& matrix) { - std::cout << label << " (" << matrix.rows() << "x" << matrix.cols() << ")" << std::endl; - std::cout << std::fixed << std::setprecision(6) << matrix << std::endl << std::endl; -} - -// Helper function to print the Hessian tensor -void printHessianTensor(const std::string& label, const std::vector& hessian) { - std::cout << label << " (Tensor with " << hessian.size() << " matrices)" << std::endl; - - for (size_t i = 0; i < hessian.size(); ++i) { - std::cout << "Matrix for state dimension " << i << " (" - << hessian[i].rows() << "x" << hessian[i].cols() << "):" << std::endl; - std::cout << std::fixed << std::setprecision(6) << hessian[i] << std::endl; - } - std::cout << std::endl; -} - -// Function to demonstrate pendulum Hessian -void demonstratePendulumHessian() { - std::cout << "\n========== PENDULUM MODEL EXAMPLE ==========" << std::endl; - - // Create a pendulum model - double timestep = 0.01; - double length = 1.0; // Length of the pendulum [m] - double mass = 1.0; // Mass [kg] - double damping = 0.1; // Damping coefficient - std::string integration = "rk4"; - - Pendulum pendulum(timestep, length, mass, damping, integration); - - // Define a state and control - Eigen::VectorXd state = Eigen::VectorXd::Zero(2); - state << M_PI / 4.0, 0.0; // 45-degree angle, zero velocity - - Eigen::VectorXd control = Eigen::VectorXd::Zero(1); - control << 1.0; // Apply a torque of 1 Nm - - // Print system information - std::cout << "Pendulum parameters:" << std::endl; - std::cout << "Length: " << pendulum.getLength() << " m" << std::endl; - std::cout << "Mass: " << pendulum.getMass() << " kg" << std::endl; - std::cout << "Damping: " << pendulum.getDamping() << std::endl; - std::cout << "Gravity: " << pendulum.getGravity() << " m/s²" << std::endl; - std::cout << "Timestep: " << pendulum.getTimestep() << " s" << std::endl; - std::cout << "Integration: " << pendulum.getIntegrationType() << std::endl << std::endl; - - // Print state and control - std::cout << "State: [theta, theta_dot] = [" << state.transpose() << "]" << std::endl; - std::cout << "Control: [torque] = [" << control.transpose() << "]" << std::endl << std::endl; - - // Compute dynamics - Eigen::VectorXd xdot = pendulum.getContinuousDynamics(state, control, 0.0); - std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl; - - Eigen::VectorXd next_state = pendulum.getDiscreteDynamics(state, control, 0.0); - std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl; - - // Compute Jacobians - Eigen::MatrixXd A = pendulum.getStateJacobian(state, control, 0.0); - Eigen::MatrixXd B = pendulum.getControlJacobian(state, control, 0.0); - - printMatrix("State Jacobian (A)", A); - printMatrix("Control Jacobian (B)", B); - - // Compute Hessians - std::vector state_hessian = pendulum.getStateHessian(state, control, 0.0); - std::vector control_hessian = pendulum.getControlHessian(state, control, 0.0); - std::vector cross_hessian = pendulum.getCrossHessian(state, control, 0.0); - - printHessianTensor("State Hessian (d²f/dx²)", state_hessian); - printHessianTensor("Control Hessian (d²f/du²)", control_hessian); - printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian); - - // Demonstrate how to access specific elements of the Hessian - // For example, accessing d²theta_dot/dtheta² (second derivative of theta_dot with respect to theta) - int state_idx = 1; // theta_dot is state index 1 - int wrt_state_idx1 = 0; // first derivative with respect to theta (index 0) - int wrt_state_idx2 = 0; // second derivative with respect to theta (index 0) - - double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2); - std::cout << "d²theta_dot/dtheta² = " << d2f_dx2 << std::endl; -} - -// Function to demonstrate Dubins car Hessian -void demonstrateDubinsCarHessian() { - std::cout << "\n========== DUBINS CAR MODEL EXAMPLE ==========" << std::endl; - - // Create a Dubins car model - double speed = 1.0; // Constant forward speed [m/s] - double timestep = 0.01; // Time step [s] - std::string integration = "rk4"; - - DubinsCar dubins_car(speed, timestep, integration); - - // Define a state and control - Eigen::VectorXd state = Eigen::VectorXd::Zero(3); - state << 0.0, 0.0, M_PI / 4.0; // (x, y, theta) = (0, 0, 45°) - - Eigen::VectorXd control = Eigen::VectorXd::Zero(1); - control << 0.5; // Turn rate of 0.5 rad/s - - // Print system information - std::cout << "Dubins Car parameters:" << std::endl; - std::cout << "Speed: " << speed << " m/s" << std::endl; - std::cout << "Timestep: " << dubins_car.getTimestep() << " s" << std::endl; - std::cout << "Integration: " << dubins_car.getIntegrationType() << std::endl << std::endl; - - // Print state and control - std::cout << "State: [x, y, theta] = [" << state.transpose() << "]" << std::endl; - std::cout << "Control: [omega] = [" << control.transpose() << "]" << std::endl << std::endl; - - // Compute dynamics - Eigen::VectorXd xdot = dubins_car.getContinuousDynamics(state, control, 0.0); - std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl; - - Eigen::VectorXd next_state = dubins_car.getDiscreteDynamics(state, control, 0.0); - std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl; - - // Compute Jacobians - Eigen::MatrixXd A = dubins_car.getStateJacobian(state, control, 0.0); - Eigen::MatrixXd B = dubins_car.getControlJacobian(state, control, 0.0); - - printMatrix("State Jacobian (A)", A); - printMatrix("Control Jacobian (B)", B); - - // Compute Hessians - std::vector state_hessian = dubins_car.getStateHessian(state, control, 0.0); - std::vector control_hessian = dubins_car.getControlHessian(state, control, 0.0); - std::vector cross_hessian = dubins_car.getCrossHessian(state, control, 0.0); - - printHessianTensor("State Hessian (d²f/dx²)", state_hessian); - printHessianTensor("Control Hessian (d²f/du²)", control_hessian); - printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian); - - // Demonstrate how to access specific elements of the Hessian - // For example, accessing d²x/dtheta² (second derivative of x with respect to theta) - int state_idx = 0; // x is state index 0 - int wrt_state_idx1 = 2; // first derivative with respect to theta (index 2) - int wrt_state_idx2 = 2; // second derivative with respect to theta (index 2) - - double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2); - std::cout << "d²x/dtheta² = " << d2f_dx2 << std::endl; -} - -int main() { - std::cout << "Hessian Examples for Dynamical Systems" << std::endl; - std::cout << "=====================================" << std::endl; - - // Demonstrate pendulum Hessian - demonstratePendulumHessian(); - - // Demonstrate Dubins car Hessian - demonstrateDubinsCarHessian(); - return 0; -} \ No newline at end of file diff --git a/examples/ipopt_car.cpp b/examples/ipopt_car.cpp deleted file mode 100644 index 14cd3b3b..00000000 --- a/examples/ipopt_car.cpp +++ /dev/null @@ -1,364 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -void plotCarBox(const Eigen::VectorXd &state, const Eigen::VectorXd &control, - double length, double width, const std::string &color, - axes_handle ax) -{ - double x = state(0); - double y = state(1); - double theta = state(2); - double steering = control(1); - - // Compute the car's four corners (and close the polygon) - std::vector car_x(5), car_y(5); - - // Front right - car_x[0] = x + length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[0] = y + length / 2 * sin(theta) + width / 2 * cos(theta); - - // Front left - car_x[1] = x + length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[1] = y + length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear left - car_x[2] = x - length / 2 * cos(theta) + width / 2 * sin(theta); - car_y[2] = y - length / 2 * sin(theta) - width / 2 * cos(theta); - - // Rear right - car_x[3] = x - length / 2 * cos(theta) - width / 2 * sin(theta); - car_y[3] = y - length / 2 * sin(theta) + width / 2 * cos(theta); - - // Close polygon - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - // Plot car body as a polygon line. - plot(ax, car_x, car_y, color + "-"); - - // Plot base point (center of rear axle) as a red circle. - plot(ax, std::vector{x}, std::vector{y}, "ro"); - - // Compute steering direction - double front_x = x + length / 2 * cos(theta); - double front_y = y + length / 2 * sin(theta); - double steering_length = width / 2; - double steering_angle = theta + steering; - double steering_end_x = front_x + steering_length * cos(steering_angle); - double steering_end_y = front_y + steering_length * sin(steering_angle); - - std::vector steer_x = {front_x, steering_end_x}; - std::vector steer_y = {front_y, steering_end_y}; - plot(ax, steer_x, steer_y, "g-"); -} - -int main() -{ - // Problem parameters - int state_dim = 4; // [x y theta v] - int control_dim = 2; // [steering_angle acceleration] - int horizon = 500; // Same as CDDP example - double timestep = 0.03; - double wheelbase = 2.0; - - // Initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << 1.0, 1.0, 1.5 * M_PI, 0.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, 0.0, 0.0; - - // Define variables - casadi::MX x = casadi::MX::sym("x", state_dim); - casadi::MX u = casadi::MX::sym("u", control_dim); - - // Car dynamics - casadi::MX theta = x(2); - casadi::MX v = x(3); - casadi::MX delta = u(0); - casadi::MX a = u(1); - - // Continuous dynamics - casadi::MX dx = casadi::MX::zeros(state_dim); - using casadi::cos; - using casadi::sin; - using casadi::tan; - - dx(0) = v * cos(theta); - dx(1) = v * sin(theta); - dx(2) = v * tan(delta) / wheelbase; - dx(3) = a; - - // Discrete dynamics using Euler integration - casadi::MX f = x + timestep * dx; - casadi::Function F("F", {x, u}, {f}); - - // Decision variables - int n_states = (horizon + 1) * state_dim; - int n_controls = horizon * control_dim; - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Objective function terms - auto running_cost = [&](casadi::MX x, casadi::MX u) - { - casadi::MX u_cost = 1e-2 * u(0) * u(0) + 1e-4 * u(1) * u(1); - casadi::MX xy_norm = x(0) * x(0) + x(1) * x(1); - casadi::MX x_cost = 1e-3 * (casadi::MX::sqrt(xy_norm / 0.01 + 1) * 0.1 - 0.1); - return u_cost + x_cost; - }; - - auto terminal_cost = [&](casadi::MX x) - { - casadi::MX cost = 0; - casadi::MX d = x - casadi::DM(std::vector(goal_state.data(), goal_state.data() + state_dim)); - casadi::MX xy_err = d(0) * d(0) + d(1) * d(1); - cost += 0.1 * (casadi::MX::sqrt(xy_err / 0.01 + 1) * 0.1 - 0.1); - cost += 1.0 * (casadi::MX::sqrt(d(2) * d(2) / 0.01 + 1) * 0.1 - 0.1); - cost += 0.3 * (casadi::MX::sqrt(d(3) * d(3) / 1.0 + 1) * 1.0 - 1.0); - return cost; - }; - - // Build objective - casadi::MX J = 0; - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - J += running_cost(x_t, u_t); - } - J += terminal_cost(X(casadi::Slice(horizon * state_dim, (horizon + 1) * state_dim))); - - // Constraints - casadi::MX g; - - // Initial condition - g = casadi::MX::vertcat({g, X(casadi::Slice(0, state_dim)) - - casadi::DM(std::vector(initial_state.data(), initial_state.data() + state_dim))}); - - // Dynamics constraints - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - casadi::MX x_next = F(casadi::MXVector{x_t, u_t})[0]; - casadi::MX x_next_sym = X(casadi::Slice((t + 1) * state_dim, (t + 2) * state_dim)); - g = casadi::MX::vertcat({g, x_next_sym - x_next}); - } - - // NLP - casadi::MXDict nlp = { - {"x", z}, - {"f", J}, - {"g", g}}; - - // Solver options - casadi::Dict solver_opts; - solver_opts["ipopt.max_iter"] = 1000; - solver_opts["ipopt.print_level"] = 5; - solver_opts["print_time"] = true; - solver_opts["ipopt.tol"] = 1e-6; - - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Bounds - std::vector lbx(n_states + n_controls); - std::vector ubx(n_states + n_controls); - std::vector lbg(g.size1()); - std::vector ubg(g.size1()); - - // State bounds - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - lbx[t * state_dim + i] = -1e20; - ubx[t * state_dim + i] = 1e20; - } - } - - // Control bounds - for (int t = 0; t < horizon; t++) - { - // Steering angle bounds - lbx[n_states + t * control_dim] = -0.5; - ubx[n_states + t * control_dim] = 0.5; - // Acceleration bounds - lbx[n_states + t * control_dim + 1] = -2.0; - ubx[n_states + t * control_dim + 1] = 2.0; - } - - // Constraint bounds (all equality constraints) - for (int i = 0; i < g.size1(); i++) - { - lbg[i] = 0; - ubg[i] = 0; - } - - // Initial guess - std::vector x0(n_states + n_controls, 0.0); - - // Initial state - for (int i = 0; i < state_dim; i++) - { - x0[i] = initial_state(i); - } - - // Linear interpolation for states - for (int t = 1; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - x0[t * state_dim + i] = initial_state(i) + - (goal_state(i) - initial_state(i)) * t / horizon; - } - } - - // Call the solver - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(casadi::DMDict{ - {"x0", x0}, - {"lbx", lbx}, - {"ubx", ubx}, - {"lbg", lbg}, - {"ubg", ubg}}); - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_time - start_time); - - std::cout << "Solve time: " << duration.count() << " microseconds" << std::endl; - - // EXIT: Optimal Solution Found. - // solver : t_proc (avg) t_wall (avg) n_eval - // nlp_f | 23.27ms (217.46us) 23.44ms (219.05us) 107 - // nlp_g | 27.51ms (257.09us) 27.57ms (257.65us) 107 - // nlp_grad_f | 39.72ms (374.68us) 39.87ms (376.14us) 106 - // nlp_hess_l | 244.66ms ( 2.35ms) 245.20ms ( 2.36ms) 104 - // nlp_jac_g | 176.39ms ( 1.66ms) 176.76ms ( 1.67ms) 106 - // total | 1.49 s ( 1.49 s) 1.49 s ( 1.49 s) 1 - // Solve time: 1488881 microseconds - - // Extract solution - std::vector sol = std::vector(res.at("x")); - - // Convert to state and control trajectories - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - X_sol[t](i) = sol[t * state_dim + i]; - } - } - - for (int t = 0; t < horizon; t++) - { - for (int i = 0; i < control_dim; i++) - { - U_sol[t](i) = sol[n_states + t * control_dim + i]; - } - } - - // Prepare trajectory data - std::vector x_hist, y_hist; - for (const auto &x : X_sol) - { - x_hist.push_back(x(0)); - y_hist.push_back(x(1)); - } - - // Car dimensions. - double car_length = 2.1; - double car_width = 0.9; - - // Create a figure and get current axes. - auto fig = figure(true); - auto ax = fig->current_axes(); - - Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(2); - - // Create directory for saving plots - const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } - - // Create a directory for frame images. - (void) std::system("mkdir -p frames"); - - // Animation loop: update plot for each time step and save frame. - for (size_t i = 0; i < X_sol.size(); ++i) - { - // Skip every 10th frame for smoother animation. - if (i % 10 == 0) - { - // Clear previous content. - cla(ax); - hold(ax, true); - - // Plot the full trajectory. - plot(ax, x_hist, y_hist, "b-"); - - // Plot goal configuration. - plotCarBox(goal_state, empty_control, car_length, car_width, "r", ax); - - // Plot current car state. - if (i < U_sol.size()) - plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k", ax); - else - plotCarBox(X_sol[i], empty_control, car_length, car_width, "k", ax); - - // Set grid and axis limits. - grid(ax, true); - xlim(ax, {-4, 4}); - ylim(ax, {-4, 4}); - - // Update drawing. - fig->draw(); - - // Save the frame to a PNG file. - std::string frame_filename = plotDirectory + "/frame_" + std::to_string(i) + ".png"; - fig->save(frame_filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 15 " + plotDirectory + "/frame_*.png " + plotDirectory + "/car_parking_ipopt.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as car_parking_ipopt.gif" << std::endl; - - return 0; -} \ No newline at end of file diff --git a/examples/ipopt_cartpole.cpp b/examples/ipopt_cartpole.cpp deleted file mode 100644 index 172aa9d8..00000000 --- a/examples/ipopt_cartpole.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -int main() { - // Dimensions and time parameters - const int state_dim = 4; // [x, θ, x_dot, θ_dot] - const int control_dim = 1; // [force] - const int horizon = 100; // Number of time steps - const double timestep = 0.05; // Time discretization - - // Cartpole physical parameters - double cart_mass = 1.0; // mass of the cart (kg) - double pole_mass = 0.2; // mass of the pole (kg) - double pole_length = 0.5; // length of the pole (m) - double gravity = 9.81; // gravitational acceleration (m/s²) - - // Define initial and goal states. - // State order: [x, θ, x_dot, θ_dot] - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, 0.0, 0.0; // Cart at origin; pole hanging downward (θ = 0) - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, M_PI, 0.0, 0.0; // Cart at origin; pole upright (θ = π) - - // Cost weighting matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf(0,0) = 100.0; // Cart position - Qf(1,1) = 100.0; // Pole angle - Qf(2,2) = 100.0; // Cart velocity - Qf(3,3) = 100.0; // Pole angular velocity - - // Convert Eigen matrices to CasADi DM using a nested loop conversion. - casadi::DM Q_dm(Q.rows(), Q.cols()); - for (int i = 0; i < Q.rows(); i++) { - for (int j = 0; j < Q.cols(); j++) { - Q_dm(i, j) = Q(i, j); - } - } - casadi::DM R_dm(R.rows(), R.cols()); - for (int i = 0; i < R.rows(); i++) { - for (int j = 0; j < R.cols(); j++) { - R_dm(i, j) = R(i, j); - } - } - casadi::DM Qf_dm(Qf.rows(), Qf.cols()); - for (int i = 0; i < Qf.rows(); i++) { - for (int j = 0; j < Qf.cols(); j++) { - Qf_dm(i, j) = Qf(i, j); - } - } - - // Control bounds (e.g., force limits) - Eigen::VectorXd control_lower_bound(control_dim); - Eigen::VectorXd control_upper_bound(control_dim); - control_lower_bound << -5.0; - control_upper_bound << 5.0; - - ////////// Decision Variables ////////// - const int n_states = (horizon + 1) * state_dim; - const int n_controls = horizon * control_dim; - const int n_dec = n_states + n_controls; - - // Define symbolic variables for states and controls - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Helper lambdas to extract the state and control at time step t - auto X_t = [=](int t) -> casadi::MX { - return X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - }; - auto U_t = [=](int t) -> casadi::MX { - return U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - }; - - auto cartpole_dynamics = [=](casadi::MX state, casadi::MX control) -> casadi::MX { - // Extract state components - casadi::MX pos = state(0); // cart position - casadi::MX theta = state(1); // pole angle - casadi::MX x_dot = state(2); - casadi::MX theta_dot = state(3); - casadi::MX force = control(0); - - // Use casadi's trigonometric functions - using casadi::cos; - using casadi::sin; - casadi::MX sin_theta = sin(theta); - casadi::MX cos_theta = cos(theta); - casadi::MX total_mass = cart_mass + pole_mass; - casadi::MX den = cart_mass + pole_mass * sin_theta * sin_theta; - - casadi::MX x_ddot = (force + pole_mass * sin_theta * (pole_length * theta_dot * theta_dot + gravity * cos_theta)) / den; - casadi::MX theta_ddot = (-force * cos_theta - - pole_mass * pole_length * theta_dot * theta_dot * cos_theta * sin_theta - - total_mass * gravity * sin_theta) / (pole_length * den); - - // Euler discretization: - casadi::MX next_state = casadi::MX::vertcat({ - pos + timestep * x_dot, - theta + timestep * theta_dot, - x_dot + timestep * x_ddot, - theta_dot + timestep * theta_ddot - }); - return next_state; - }; - - ////////// Constraints ////////// - casadi::MX g; - - // Initial state constraint: - casadi::DM init_state_dm(std::vector(initial_state.data(), initial_state.data() + state_dim)); - g = casadi::MX::vertcat({g, X_t(0) - init_state_dm}); - - // Dynamics constraints: - for (int t = 0; t < horizon; t++) { - casadi::MX x_next_expr = cartpole_dynamics(X_t(t), U_t(t)); - g = casadi::MX::vertcat({g, X_t(t + 1) - x_next_expr}); - } - - // --- Terminal Condition Constraint --- - casadi::DM goal_dm(std::vector(goal_state.data(), goal_state.data() + state_dim)); - casadi::MX terminal_constr = X_t(horizon) - goal_dm; - g = casadi::MX::vertcat({g, terminal_constr}); - - ////////// Cost Function ////////// - casadi::MX cost = casadi::MX::zeros(1, 1); - for (int t = 0; t < horizon; t++) { - casadi::MX x_diff = X_t(t) - goal_dm; - casadi::MX u_diff = U_t(t); - casadi::MX state_cost = casadi::MX::mtimes({x_diff.T(), Q_dm, x_diff}) * timestep; - casadi::MX control_cost = casadi::MX::mtimes({u_diff.T(), R_dm, u_diff}) * timestep; - cost = cost + state_cost + control_cost; - } - - // Terminal cost - casadi::MX x_diff_final = X_t(horizon) - goal_dm; - casadi::MX terminal_cost = casadi::MX::mtimes({x_diff_final.T(), Qf_dm, x_diff_final}); - cost = cost + terminal_cost; - - std::vector lbx(n_dec, -1e20), ubx(n_dec, 1e20); - // Apply control bounds to the control part of the decision vector. - for (int t = 0; t < horizon; t++) { - for (int i = 0; i < control_dim; i++) { - lbx[n_states + t * control_dim + i] = control_lower_bound(i); - ubx[n_states + t * control_dim + i] = control_upper_bound(i); - } - } - - // The complete set of constraints (g) must be equal to zero. - const int n_g = static_cast(g.size1()); - std::vector lbg(n_g, 0.0); - std::vector ubg(n_g, 0.0); - - // Provide an initial guess for the decision vector. - std::vector x0(n_dec, 0.0); - // Set the initial state portion. - for (int i = 0; i < state_dim; i++) { - x0[i] = initial_state(i); - } - // // Linearly interpolate the state trajectory from initial_state to goal_state. - // for (int t = 1; t <= horizon; t++) { - // for (int i = 0; i < state_dim; i++) { - // x0[t * state_dim + i] = initial_state(i) + (goal_state(i) - initial_state(i)) * (double)t / horizon; - // } - // } - - std::map nlp; - nlp["x"] = z; - nlp["f"] = cost; - nlp["g"] = g; - - casadi::Dict solver_opts; - solver_opts["print_time"] = true; - solver_opts["ipopt.print_level"] = 5; - solver_opts["ipopt.max_iter"] = 500; - solver_opts["ipopt.tol"] = 1e-6; - - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Convert initial guess and bounds into DM objects. - casadi::DM x0_dm = casadi::DM(x0); - casadi::DM lbx_dm = casadi::DM(lbx); - casadi::DM ubx_dm = casadi::DM(ubx); - casadi::DM lbg_dm = casadi::DM(lbg); - casadi::DM ubg_dm = casadi::DM(ubg); - - casadi::DMDict arg({ - {"x0", x0_dm}, - {"lbx", lbx_dm}, - {"ubx", ubx_dm}, - {"lbg", lbg_dm}, - {"ubg", ubg_dm} - }); - - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(arg); - auto end_time = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end_time - start_time; - std::cout << "Solver elapsed time: " << elapsed.count() << " s" << std::endl; - - std::vector sol = std::vector(res.at("x")); - - // Recover state and control trajectories. - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - for (int t = 0; t <= horizon; t++) { - Eigen::VectorXd x_t(state_dim); - for (int i = 0; i < state_dim; i++) { - x_t(i) = sol[t * state_dim + i]; - } - X_sol[t] = x_t; - } - for (int t = 0; t < horizon; t++) { - Eigen::VectorXd u_t(control_dim); - for (int i = 0; i < control_dim; i++) { - u_t(i) = sol[n_states + t * control_dim + i]; - } - U_sol[t] = u_t; - } - - // Create plot directory. - const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directory(plotDirectory); - } - - // Create a time vector. - std::vector t_sol(horizon + 1); - for (int t = 0; t <= horizon; t++) { - t_sol[t] = t * timestep; - } - - // Extract solution data. - std::vector x_arr, x_dot_arr, theta_arr, theta_dot_arr, force_arr, time_arr, time_arr2; - for (size_t i = 0; i < X_sol.size(); ++i) { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - theta_arr.push_back(X_sol[i](1)); - x_dot_arr.push_back(X_sol[i](2)); - theta_dot_arr.push_back(X_sol[i](3)); - } - for (size_t i = 0; i < U_sol.size(); ++i) { - force_arr.push_back(U_sol[i](0)); - time_arr2.push_back(t_sol[i]); - } - - // --- Plot static results (2x2 plots for state trajectories) --- - auto fig1 = figure(); - fig1->size(1200, 800); - - auto ax1 = subplot(2, 2, 1); - title(ax1, "Cart Position"); - plot(ax1, time_arr, x_arr)->line_style("b-"); - xlabel(ax1, "Time [s]"); - ylabel(ax1, "Position [m]"); - grid(ax1, true); - - auto ax2 = subplot(2, 2, 2); - title(ax2, "Cart Velocity"); - plot(ax2, time_arr, x_dot_arr)->line_style("b-"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "Velocity [m/s]"); - grid(ax2, true); - - auto ax3 = subplot(2, 2, 3); - title(ax3, "Pole Angle"); - plot(ax3, time_arr, theta_arr)->line_style("b-"); - xlabel(ax3, "Time [s]"); - ylabel(ax3, "Angle [rad]"); - grid(ax3, true); - - auto ax4 = subplot(2, 2, 4); - title(ax4, "Pole Angular Velocity"); - plot(ax4, time_arr, theta_dot_arr)->line_style("b-"); - xlabel(ax4, "Time [s]"); - ylabel(ax4, "Angular Velocity [rad/s]"); - grid(ax4, true); - - fig1->save(plotDirectory + "/cartpole_ipopt_results.png"); - - // --- Plot control inputs --- - auto fig2 = figure(); - fig2->size(800, 600); - title("Control Inputs"); - plot(time_arr2, force_arr)->line_style("b-"); - xlabel("Time [s]"); - ylabel("Force [N]"); - grid(true); - fig2->save(plotDirectory + "/cartpole_ipopt_control_inputs.png"); - - // --- Animation --- - auto fig3 = figure(); - auto ax_fig3 = fig3->current_axes(); - fig3->size(800, 600); - title("CartPole Animation"); - xlabel("x"); - ylabel("y"); - - double cart_width = 0.3; - double cart_height = 0.2; - double pole_width = 0.05; - - // Loop over the solution states to generate animation frames. - for (size_t i = 0; i < X_sol.size(); ++i) { - if (i % 5 == 0) { - // Clear previous content. - cla(ax_fig3); - hold(ax_fig3, true); - - // Current state. - double x = x_arr[i]; - double theta = theta_arr[i]; - - // Plot the cart as a rectangle centered at (x, 0). - std::vector cart_x = { x - cart_width/2, x + cart_width/2, - x + cart_width/2, x - cart_width/2, - x - cart_width/2 }; - std::vector cart_y = { -cart_height/2, -cart_height/2, - cart_height/2, cart_height/2, - -cart_height/2 }; - plot(cart_x, cart_y)->line_style("k-"); - - // Plot the pole as a line from the top center of the cart. - double pole_end_x = x + pole_length * std::sin(theta); - double pole_end_y = cart_height/2 - pole_length * std::cos(theta); - std::vector pole_x = { x, pole_end_x }; - std::vector pole_y = { cart_height/2, pole_end_y }; - plot(pole_x, pole_y)->line_style("b-"); - - // Plot the pole bob as a circle. - std::vector circle_x, circle_y; - int num_points = 20; - for (int j = 0; j <= num_points; ++j) { - double t = 2 * M_PI * j / num_points; - circle_x.push_back(pole_end_x + pole_width * std::cos(t)); - circle_y.push_back(pole_end_y + pole_width * std::sin(t)); - } - plot(circle_x, circle_y)->line_style("b-"); - - // Set fixed axis limits for stable animation. - xlim({-2.0, 2.0}); - ylim({-1.5, 1.5}); - - // Save the frame. - std::string filename = plotDirectory + "/frame_" + std::to_string(i) + ".png"; - fig3->save(filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // Combine all saved frames into a GIF using ImageMagick's convert tool. - std::string command = "convert -delay 30 " + plotDirectory + "/frame_*.png " + plotDirectory + "/cartpole_ipopt.gif"; - std::system(command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "Animation saved as cartpole_ipopt.gif" << std::endl; - - return 0; -} diff --git a/examples/ipopt_quadrotor.cpp b/examples/ipopt_quadrotor.cpp deleted file mode 100644 index c6365eba..00000000 --- a/examples/ipopt_quadrotor.cpp +++ /dev/null @@ -1,694 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -// Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) -Eigen::Vector3d quaternionToEuler(double qw, double qx, double qy, double qz) -{ - // Roll (phi) - double sinr_cosp = 2.0 * (qw * qx + qy * qz); - double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); - double phi = std::atan2(sinr_cosp, cosr_cosp); - - // Pitch (theta) - double sinp = 2.0 * (qw * qy - qz * qx); - double theta = (std::abs(sinp) >= 1.0) ? std::copysign(M_PI / 2.0, sinp) : std::asin(sinp); - - // Yaw (psi) - double siny_cosp = 2.0 * (qw * qz + qx * qy); - double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); - double psi = std::atan2(siny_cosp, cosy_cosp); - - return Eigen::Vector3d(phi, theta, psi); -} - -// Compute rotation matrix from a unit quaternion [qw, qx, qy, qz] -Eigen::Matrix3d getRotationMatrixFromQuaternion(double qw, double qx, double qy, double qz) -{ - Eigen::Matrix3d R; - R(0, 0) = 1 - 2 * (qy * qy + qz * qz); - R(0, 1) = 2 * (qx * qy - qz * qw); - R(0, 2) = 2 * (qx * qz + qy * qw); - - R(1, 0) = 2 * (qx * qy + qz * qw); - R(1, 1) = 1 - 2 * (qx * qx + qz * qz); - R(1, 2) = 2 * (qy * qz - qx * qw); - - R(2, 0) = 2 * (qx * qz - qy * qw); - R(2, 1) = 2 * (qy * qz + qx * qw); - R(2, 2) = 1 - 2 * (qx * qx + qy * qy); - return R; -} - -int main() { - ////////// Problem Setup ////////// - const int state_dim = 13; // [x, y, z, qw, qx, qy, qz, vx, vy, vz, omega_x, omega_y, omega_z] - const int control_dim = 4; // [f1, f2, f3, f4] - const int horizon = 400; // Number of control intervals - const double timestep = 0.02; // Time step - - // Quadrotor parameters - const double mass = 1.2; // kg - const double arm_length = 0.165; // m - const double gravity = 9.81; // m/s^2 - - // Inertia matrix - Eigen::Matrix3d inertia_matrix = Eigen::Matrix3d::Zero(); - inertia_matrix(0, 0) = 7.782e-3; // Ixx - inertia_matrix(1, 1) = 7.782e-3; // Iyy - inertia_matrix(2, 2) = 1.439e-2; // Izz - - // Figure-8 trajectory parameters - double figure8_scale = 3.0; // 3m - double constant_altitude = 2.0; // 2m - double total_time = horizon * timestep; - double omega = 2.0 * M_PI / total_time; // completes 1 cycle over the horizon - - // Define initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << figure8_scale, 0.0, constant_altitude, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << figure8_scale, 0.0, constant_altitude, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - - // Generate figure-8 reference trajectory - std::vector figure8_reference_states; - figure8_reference_states.reserve(horizon + 1); - - for (int i = 0; i <= horizon; ++i) { - double t = i * timestep; - double angle = omega * t; - - Eigen::VectorXd ref_state = Eigen::VectorXd::Zero(state_dim); - ref_state(0) = figure8_scale * std::cos(angle); // x - ref_state(1) = figure8_scale * std::sin(angle) * std::cos(angle); // y - ref_state(2) = constant_altitude; // z - ref_state(3) = 1.0; // qw (identity quaternion) - ref_state(4) = 0.0; // qx - ref_state(5) = 0.0; // qy - ref_state(6) = 0.0; // qz - - figure8_reference_states.push_back(ref_state); - } - - // Define cost weighting matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - Q(0, 0) = 1.0; // x position - Q(1, 1) = 1.0; // y position - Q(2, 2) = 1.0; // z position - - Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - // Terminal cost weight - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(state_dim, state_dim); - Qf(0, 0) = 1.0; - Qf(1, 1) = 1.0; - Qf(2, 2) = 1.0; - Qf(3, 3) = 1.0; - Qf(4, 4) = 1.0; - Qf(5, 5) = 1.0; - Qf(6, 6) = 1.0; - - // Convert Eigen matrices to CasADi - casadi::DM Q_dm(Q.rows(), Q.cols()); - for (int i = 0; i < Q.rows(); i++) { - for (int j = 0; j < Q.cols(); j++) { - Q_dm(i, j) = Q(i, j) * timestep; - } - } - casadi::DM R_dm(R.rows(), R.cols()); - for (int i = 0; i < R.rows(); i++) { - for (int j = 0; j < R.cols(); j++) { - R_dm(i, j) = R(i, j) * timestep; - } - } - casadi::DM Qf_dm(Qf.rows(), Qf.cols()); - for (int i = 0; i < Qf.rows(); i++) { - for (int j = 0; j < Qf.cols(); j++) { - Qf_dm(i, j) = Qf(i, j); - } - } - - // Convert inertia matrix to CasADi - casadi::DM inertia_dm(3, 3); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - inertia_dm(i, j) = inertia_matrix(i, j); - } - } - - // Define control bounds - Eigen::VectorXd u_min(control_dim), u_max(control_dim); - u_min << 0.0, 0.0, 0.0, 0.0; - u_max << 4.0, 4.0, 4.0, 4.0; - - const int n_states = (horizon + 1) * state_dim; - const int n_controls = horizon * control_dim; - const int n_dec = n_states + n_controls; - - // Define symbolic variables for states and controls - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Helper lambdas to extract the state and control at time step t - auto X_t = [=](int t) -> casadi::MX { - return X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - }; - auto U_t = [=](int t) -> casadi::MX { - return U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - }; - - // Quadrotor continuous dynamics function (computes derivatives) - auto quadrotor_derivatives = [=](casadi::MX x, casadi::MX u) -> casadi::MX { - casadi::MX x_dot = casadi::MX::zeros(state_dim, 1); - - // Extract states - casadi::MX pos = x(casadi::Slice(0, 3)); // [x, y, z] - casadi::MX quat = x(casadi::Slice(3, 7)); // [qw, qx, qy, qz] - casadi::MX vel = x(casadi::Slice(7, 10)); // [vx, vy, vz] - casadi::MX omega = x(casadi::Slice(10, 13)); // [omega_x, omega_y, omega_z] - - casadi::MX qw = quat(0), qx = quat(1), qy = quat(2), qz = quat(3); - casadi::MX omega_x = omega(0), omega_y = omega(1), omega_z = omega(2); - - // Normalize quaternion - casadi::MX q_norm = casadi::MX::sqrt(qw*qw + qx*qx + qy*qy + qz*qz); - qw = qw / q_norm; - qx = qx / q_norm; - qy = qy / q_norm; - qz = qz / q_norm; - - // Extract control inputs (motor forces) - casadi::MX f1 = u(0), f2 = u(1), f3 = u(2), f4 = u(3); - - // Compute total thrust and moments - casadi::MX thrust = f1 + f2 + f3 + f4; - casadi::MX tau_x = arm_length * (f1 - f3); - casadi::MX tau_y = arm_length * (f2 - f4); - casadi::MX tau_z = 0.1 * (f1 - f2 + f3 - f4); - - // Rotation matrix from quaternion - casadi::MX R11 = 1 - 2 * (qy * qy + qz * qz); - casadi::MX R12 = 2 * (qx * qy - qz * qw); - casadi::MX R13 = 2 * (qx * qz + qy * qw); - casadi::MX R21 = 2 * (qx * qy + qz * qw); - casadi::MX R22 = 1 - 2 * (qx * qx + qz * qz); - casadi::MX R23 = 2 * (qy * qz - qx * qw); - casadi::MX R31 = 2 * (qx * qz - qy * qw); - casadi::MX R32 = 2 * (qy * qz + qx * qw); - casadi::MX R33 = 1 - 2 * (qx * qx + qy * qy); - - // Position derivative = velocity - x_dot(casadi::Slice(0, 3)) = vel; - - // Quaternion derivative - x_dot(3) = -0.5 * (qx * omega_x + qy * omega_y + qz * omega_z); // qw_dot - x_dot(4) = 0.5 * (qw * omega_x + qy * omega_z - qz * omega_y); // qx_dot - x_dot(5) = 0.5 * (qw * omega_y - qx * omega_z + qz * omega_x); // qy_dot - x_dot(6) = 0.5 * (qw * omega_z + qx * omega_y - qy * omega_x); // qz_dot - - // Velocity derivative (thrust is applied along body z-axis) - casadi::MX thrust_world_x = R13 * thrust; - casadi::MX thrust_world_y = R23 * thrust; - casadi::MX thrust_world_z = R33 * thrust; - - x_dot(7) = thrust_world_x / mass; // vx_dot - x_dot(8) = thrust_world_y / mass; // vy_dot - x_dot(9) = thrust_world_z / mass - gravity; // vz_dot - - // Angular velocity derivative - casadi::MX inertia_inv = casadi::MX::inv(inertia_dm); - casadi::MX tau_vec = casadi::MX::vertcat({tau_x, tau_y, tau_z}); - casadi::MX gyroscopic = casadi::MX::cross(omega, casadi::MX::mtimes(inertia_dm, omega)); - casadi::MX angular_acc = casadi::MX::mtimes(inertia_inv, tau_vec - gyroscopic); - - x_dot(casadi::Slice(10, 13)) = angular_acc; - - return x_dot; - }; - - // RK4 integration function - auto quadrotor_dynamics = [=](casadi::MX x, casadi::MX u) -> casadi::MX { - // RK4 integration: k1, k2, k3, k4 - casadi::MX k1 = quadrotor_derivatives(x, u); - casadi::MX k2 = quadrotor_derivatives(x + timestep/2.0 * k1, u); - casadi::MX k3 = quadrotor_derivatives(x + timestep/2.0 * k2, u); - casadi::MX k4 = quadrotor_derivatives(x + timestep * k3, u); - - // RK4 final integration step - return x + timestep/6.0 * (k1 + 2.0*k2 + 2.0*k3 + k4); - }; - - casadi::MX g; - - // Initial state constraint: X₀ = initial_state - casadi::DM init_state_dm(std::vector(initial_state.data(), initial_state.data() + state_dim)); - g = casadi::MX::vertcat({g, X_t(0) - init_state_dm}); - - // Dynamics constraints - for (int t = 0; t < horizon; t++) { - casadi::MX x_next_expr = quadrotor_dynamics(X_t(t), U_t(t)); - g = casadi::MX::vertcat({g, X_t(t + 1) - x_next_expr}); - } - - ////////// Cost Function ////////// - casadi::MX cost = casadi::MX::zeros(1, 1); - - // Running cost - for (int t = 0; t < horizon; t++) { - // Convert reference state to CasADi - casadi::DM ref_dm(std::vector(figure8_reference_states[t].data(), - figure8_reference_states[t].data() + state_dim)); - - casadi::MX x_diff = X_t(t) - ref_dm; - casadi::MX u_diff = U_t(t); - - casadi::MX state_cost = casadi::MX::mtimes({x_diff.T(), Q_dm, x_diff}); - casadi::MX control_cost = casadi::MX::mtimes({u_diff.T(), R_dm, u_diff}); - cost = cost + state_cost + control_cost; - } - - // Terminal cost - casadi::DM goal_dm(std::vector(goal_state.data(), goal_state.data() + state_dim)); - casadi::MX x_diff_final = X_t(horizon) - goal_dm; - casadi::MX terminal_cost = casadi::MX::mtimes({x_diff_final.T(), Qf_dm, x_diff_final}); - cost = cost + terminal_cost; - - ////////// Variable Bounds and Initial Guess ////////// - std::vector lbx(n_dec, -1e20); - std::vector ubx(n_dec, 1e20); - - // Apply control bounds - for (int t = 0; t < horizon; t++) { - for (int i = 0; i < control_dim; i++) { - lbx[n_states + t * control_dim + i] = u_min(i); - ubx[n_states + t * control_dim + i] = u_max(i); - } - } - - // The complete set of constraints (g) must be equal to zero - const int n_g = static_cast(g.size1()); - std::vector lbg(n_g, 0.0); - std::vector ubg(n_g, 0.0); - - // Provide an initial guess for the decision vector - std::vector x0(n_dec, 0.0); - - // Set the initial state portion - for (int i = 0; i < state_dim; i++) { - - x0[i] = initial_state(i); - } - - // Use the reference trajectory as initial guess for states - for (int t = 1; t <= horizon; t++) { - for (int i = 0; i < state_dim; i++) { - x0[t * state_dim + i] = figure8_reference_states[t](i); - } - } - - // Initial guess for controls (hover thrust) - double hover_thrust = mass * gravity / 4.0; - for (int t = 0; t < horizon; t++) { - for (int i = 0; i < control_dim; i++) { - x0[n_states + t * control_dim + i] = hover_thrust; - } - } - - ////////// NLP Definition and IPOPT Solver Setup ////////// - std::map nlp; - nlp["x"] = z; - nlp["f"] = cost; - nlp["g"] = g; - - casadi::Dict solver_opts; - solver_opts["print_time"] = true; - solver_opts["ipopt.print_level"] = 5; - solver_opts["ipopt.max_iter"] = 1000; - solver_opts["ipopt.tol"] = 1e-6; - solver_opts["ipopt.acceptable_tol"] = 1e-4; - - // Create the NLP solver instance using IPOPT - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Convert the initial guess and bounds into DM objects - casadi::DM x0_dm = casadi::DM(x0); - casadi::DM lbx_dm = casadi::DM(lbx); - casadi::DM ubx_dm = casadi::DM(ubx); - casadi::DM lbg_dm = casadi::DM(lbg); - casadi::DM ubg_dm = casadi::DM(ubg); - - casadi::DMDict arg({ - {"x0", x0_dm}, - {"lbx", lbx_dm}, - {"ubx", ubx_dm}, - {"lbg", lbg_dm}, - {"ubg", ubg_dm} - }); - - ////////// Solve the NLP ////////// - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(arg); - auto end_time = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end_time - start_time; - std::cout << "Solver elapsed time: " << elapsed.count() << " s" << std::endl; - - ////////// Extract and Display the Solution ////////// - std::vector sol = std::vector(res.at("x")); - - // Convert to state and control trajectories - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - std::vector t_sol(horizon + 1); - for (int t = 0; t <= horizon; t++) { - t_sol[t] = t * timestep; - } - - for (int t = 0; t <= horizon; t++) { - for (int i = 0; i < state_dim; i++) { - X_sol[t](i) = sol[t * state_dim + i]; - } - } - - for (int t = 0; t < horizon; t++) { - for (int i = 0; i < control_dim; i++) { - U_sol[t](i) = sol[n_states + t * control_dim + i]; - } - } - - // Create directory for saving plot (if it doesn't exist) - const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directory(plotDirectory); - } - - // Extract trajectory data for plotting - std::vector time_arr, x_arr, y_arr, z_arr; - std::vector phi_arr, theta_arr, psi_arr; - std::vector qw_arr, qx_arr, qy_arr, qz_arr, q_norm_arr; - - for (size_t i = 0; i < X_sol.size(); ++i) { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - y_arr.push_back(X_sol[i](1)); - z_arr.push_back(X_sol[i](2)); - - double qw = X_sol[i](3); - double qx = X_sol[i](4); - double qy = X_sol[i](5); - double qz = X_sol[i](6); - - // Euler angles - Eigen::Vector3d euler = quaternionToEuler(qw, qx, qy, qz); - phi_arr.push_back(euler(0)); - theta_arr.push_back(euler(1)); - psi_arr.push_back(euler(2)); - - // Quaternion data - qw_arr.push_back(qw); - qx_arr.push_back(qx); - qy_arr.push_back(qy); - qz_arr.push_back(qz); - q_norm_arr.push_back(std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz)); - } - - // Control data - std::vector time_arr2(time_arr.begin(), time_arr.end() - 1); - std::vector f1_arr, f2_arr, f3_arr, f4_arr; - for (const auto& u : U_sol) { - f1_arr.push_back(u(0)); - f2_arr.push_back(u(1)); - f3_arr.push_back(u(2)); - f4_arr.push_back(u(3)); - } - - // ----------------------------- - // Plot states and controls - // ----------------------------- - auto f1 = figure(); - f1->size(1200, 900); - - // Position trajectories - auto ax1 = subplot(4, 1, 0); - auto plot_handle1 = plot(ax1, time_arr, x_arr, "-r"); - plot_handle1->display_name("x"); - plot_handle1->line_width(2); - hold(ax1, true); - auto plot_handle2 = plot(ax1, time_arr, y_arr, "-g"); - plot_handle2->display_name("y"); - plot_handle2->line_width(2); - auto plot_handle3 = plot(ax1, time_arr, z_arr, "-b"); - plot_handle3->display_name("z"); - plot_handle3->line_width(2); - - title(ax1, "Position Trajectories"); - xlabel(ax1, "Time [s]"); - ylabel(ax1, "Position [m]"); - matplot::legend(ax1); - grid(ax1, true); - - // Attitude angles - auto ax2 = subplot(4, 1, 1); - hold(ax2, true); - auto plot_handle4 = plot(ax2, time_arr, phi_arr, "-r"); - plot_handle4->display_name("roll"); - plot_handle4->line_width(2); - auto plot_handle5 = plot(ax2, time_arr, theta_arr, "-g"); - plot_handle5->display_name("pitch"); - plot_handle5->line_width(2); - auto plot_handle6 = plot(ax2, time_arr, psi_arr, "-b"); - plot_handle6->display_name("yaw"); - plot_handle6->line_width(2); - - title(ax2, "Attitude Angles"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "Angle [rad]"); - matplot::legend(ax2); - grid(ax2, true); - - // Motor forces - auto ax3 = subplot(4, 1, 2); - auto plot_handle7 = plot(ax3, time_arr2, f1_arr, "-r"); - plot_handle7->display_name("f1"); - plot_handle7->line_width(2); - hold(ax3, true); - auto plot_handle8 = plot(ax3, time_arr2, f2_arr, "-g"); - plot_handle8->display_name("f2"); - plot_handle8->line_width(2); - auto plot_handle9 = plot(ax3, time_arr2, f3_arr, "-b"); - plot_handle9->display_name("f3"); - plot_handle9->line_width(2); - auto plot_handle10 = plot(ax3, time_arr2, f4_arr, "-k"); - plot_handle10->display_name("f4"); - plot_handle10->line_width(2); - - title(ax3, "Motor Forces"); - xlabel(ax3, "Time [s]"); - ylabel(ax3, "Force [N]"); - matplot::legend(ax3); - grid(ax3, true); - - // Quaternion components and norm - auto ax4 = subplot(4, 1, 3); - hold(ax4, true); - - auto qwh = plot(ax4, time_arr, qw_arr, "-r"); - qwh->display_name("q_w"); - qwh->line_width(2); - - auto qxh = plot(ax4, time_arr, qx_arr, "-g"); - qxh->display_name("q_x"); - qxh->line_width(2); - - auto qyh = plot(ax4, time_arr, qy_arr, "-b"); - qyh->display_name("q_y"); - qyh->line_width(2); - - auto qzh = plot(ax4, time_arr, qz_arr, "-m"); - qzh->display_name("q_z"); - qzh->line_width(2); - - auto qnorm_handle = plot(ax4, time_arr, q_norm_arr, "-k"); - qnorm_handle->display_name("|q|"); - - title(ax4, "Quaternion Components and Norm"); - xlabel(ax4, "Time [s]"); - ylabel(ax4, "Value"); - matplot::legend(ax4); - grid(ax4, true); - - f1->draw(); - f1->save(plotDirectory + "/quadrotor_ipopt_results.png"); - - // ----------------------------- - // 3D Trajectory Plot - // ----------------------------- - auto f2 = figure(); - f2->size(800, 600); - auto ax_3d = f2->current_axes(); - hold(ax_3d, true); - - auto traj3d = plot3(ax_3d, x_arr, y_arr, z_arr); - traj3d->display_name("Trajectory"); - traj3d->line_style("-"); - traj3d->line_width(2); - traj3d->color("blue"); - - // Project trajectory onto x-y plane at z=0 - auto proj_xy = plot3(ax_3d, x_arr, y_arr, std::vector(x_arr.size(), 0.0)); - proj_xy->display_name("X-Y Projection"); - proj_xy->line_style("--"); - proj_xy->line_width(1); - proj_xy->color("gray"); - - xlabel(ax_3d, "X [m]"); - ylabel(ax_3d, "Y [m]"); - zlabel(ax_3d, "Z [m]"); - xlim(ax_3d, {-5, 5}); - ylim(ax_3d, {-2, 2}); - zlim(ax_3d, {0, 5}); - title(ax_3d, "3D Trajectory (Figure-8) - IPOPT"); - grid(ax_3d, true); - - f2->draw(); - f2->save(plotDirectory + "/quadrotor_ipopt_3d.png"); - - // ----------------------------- - // Animation: Quadrotor Trajectory - // ----------------------------- - auto f_anim = figure(); - f_anim->size(800, 600); - auto ax_anim = f_anim->current_axes(); - - // For collecting the trajectory as we go - std::vector anim_x, anim_y, anim_z; - - // Render every Nth frame to reduce #images - int frame_stride = 15; - double prop_radius = 0.03; - - for (size_t i = 0; i < X_sol.size(); i += frame_stride) { - ax_anim->clear(); - ax_anim->hold(true); - ax_anim->grid(true); - - // Current state - double x = X_sol[i](0); - double y = X_sol[i](1); - double z = X_sol[i](2); - - // Accumulate path - anim_x.push_back(x); - anim_y.push_back(y); - anim_z.push_back(z); - - // Plot the partial trajectory so far - auto path_plot = plot3(anim_x, anim_y, anim_z); - path_plot->line_width(1.5); - path_plot->line_style("--"); - path_plot->color("black"); - - // Build rotation from quaternion - Eigen::Vector4d quat(X_sol[i](3), X_sol[i](4), X_sol[i](5), X_sol[i](6)); - Eigen::Matrix3d R = getRotationMatrixFromQuaternion(quat(0), quat(1), quat(2), quat(3)); - - // Arm endpoints (front, right, back, left) - std::vector arm_endpoints; - arm_endpoints.push_back(Eigen::Vector3d(arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, arm_length, 0)); - arm_endpoints.push_back(Eigen::Vector3d(-arm_length, 0, 0)); - arm_endpoints.push_back(Eigen::Vector3d(0, -arm_length, 0)); - - // Transform to world coords - for (auto& pt : arm_endpoints) { - pt = Eigen::Vector3d(x, y, z) + R * pt; - } - - // Front-back arm - std::vector fx = {arm_endpoints[0].x(), arm_endpoints[2].x()}; - std::vector fy = {arm_endpoints[0].y(), arm_endpoints[2].y()}; - std::vector fz = {arm_endpoints[0].z(), arm_endpoints[2].z()}; - auto fb_arm = plot3(fx, fy, fz); - fb_arm->line_width(2.0); - fb_arm->color("blue"); - - // Right-left arm - std::vector rx = {arm_endpoints[1].x(), arm_endpoints[3].x()}; - std::vector ry = {arm_endpoints[1].y(), arm_endpoints[3].y()}; - std::vector rz = {arm_endpoints[1].z(), arm_endpoints[3].z()}; - auto rl_arm = plot3(rx, ry, rz); - rl_arm->line_width(2.0); - rl_arm->color("red"); - - // Motor props as small circles - auto sphere_points = linspace(0, 2 * M_PI, 15); - for (const auto& motor_pos : arm_endpoints) { - std::vector circ_x, circ_y, circ_z; - for (auto angle : sphere_points) { - circ_x.push_back(motor_pos.x() + prop_radius * cos(angle)); - circ_y.push_back(motor_pos.y() + prop_radius * sin(angle)); - circ_z.push_back(motor_pos.z()); - } - auto sphere_plot = plot3(circ_x, circ_y, circ_z); - sphere_plot->line_style("solid"); - sphere_plot->line_width(1.5); - sphere_plot->color("cyan"); - } - - title(ax_anim, "Quadrotor Animation - IPOPT"); - xlabel(ax_anim, "X [m]"); - ylabel(ax_anim, "Y [m]"); - zlabel(ax_anim, "Z [m]"); - xlim(ax_anim, {-5, 5}); - ylim(ax_anim, {-5, 5}); - zlim(ax_anim, {0, 5}); - - ax_anim->view(30, -30); - - std::string frameFile = plotDirectory + "/quadrotor_ipopt_frame_" + std::to_string(i / frame_stride) + ".png"; - f_anim->draw(); - f_anim->save(frameFile); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/quadrotor_ipopt_frame_*.png " + plotDirectory + "/quadrotor_ipopt.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/quadrotor_ipopt_frame_*.png"; - std::system(cleanup_command.c_str()); - - std::cout << "GIF animation created successfully: " << plotDirectory + "/quadrotor_ipopt.gif" << std::endl; - std::cout << "Final state = " << X_sol.back().transpose() << std::endl; - - return 0; -} diff --git a/examples/ipopt_spacecrat_linear_fuel.cpp b/examples/ipopt_spacecrat_linear_fuel.cpp deleted file mode 100644 index 452b85c4..00000000 --- a/examples/ipopt_spacecrat_linear_fuel.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -int main() -{ - // Problem parameters - int state_dim = 8; // [x y z vx vy vz mass accumulated_control_effort] - int control_dim = 3; // [ux uy uz] - int horizon = 400; - double time_horizon = 400.0; - double timestep = time_horizon / horizon; - - // HCW parameters - double mean_motion = 0.001107; - double initial_mass = 100.0; - double isp = 300.0; - double g0 = 9.80665; - double nominal_radius = 50.0; - double u_max = 10.0; - double u_min = -10.0; - double u_max_norm = 10.0; - double u_min_norm = 0.0; - - // Initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << nominal_radius, 0.0, 0.0, 0.0, -2.0*mean_motion*nominal_radius, 0.0, initial_mass, 0.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, initial_mass, 0.0; - - // Define variables - casadi::MX x = casadi::MX::sym("x", state_dim); - casadi::MX u = casadi::MX::sym("u", control_dim); - - - casadi::MX vx = x(3); - casadi::MX vy = x(4); - casadi::MX vz = x(5); - casadi::MX mass = x(6); - casadi::MX accumulated_control_effort = x(7); - casadi::MX ux = u(0); - casadi::MX uy = u(1); - casadi::MX uz = u(2); - - // Continuous dynamics (HCW) - casadi::MX dx = casadi::MX::zeros(state_dim); - - dx(0) = vx; - dx(1) = vy; - dx(2) = vz; - dx(3) = -2.0*mean_motion*vx + ux / mass; - dx(4) = -2.0*mean_motion*vy + uy / mass; - dx(5) = -2.0*mean_motion*vz + uz / mass; - dx(6) = -ux*ux - uy*uy - uz*uz / (mass*mass); - dx(7) = ux*ux + uy*uy + uz*uz; - - // Discrete dynamics using Euler integration - casadi::MX f = x + timestep * dx; - casadi::Function F("F", {x, u}, {f}); - - // Decision variables - int n_states = (horizon + 1) * state_dim; - int n_controls = horizon * control_dim; - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Objective function terms - auto running_cost = [&](casadi::MX x, casadi::MX u) - { - return 0.0; - }; - - auto terminal_cost = [&](casadi::MX x) - { - // Final cost is the terminal mass. - casadi::MX cost =-x(6); - return cost; - }; - - // Build objective (general) - casadi::MX J = 0; - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - J += running_cost(x_t, u_t); - } - J += terminal_cost(X(casadi::Slice(horizon * state_dim, (horizon + 1) * state_dim))); - - // Constraints - casadi::MX g; - - // Initial condition (general) - g = X(casadi::Slice(0, state_dim)) - - casadi::DM(std::vector(initial_state.data(), initial_state.data() + state_dim)); - - // Dynamics constraints (general) - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - casadi::MX x_next = F(casadi::MXVector{x_t, u_t})[0]; - casadi::MX x_next_sym = X(casadi::Slice((t + 1) * state_dim, (t + 2) * state_dim)); - g = casadi::MX::vertcat({g, x_next_sym - x_next}); - } - - // Terminal condition (HCW specific) - // x, y, z, vx, vy, vz are zero - casadi::MX x_T = X(casadi::Slice(horizon * state_dim, (horizon + 1) * state_dim)); - g = casadi::MX::vertcat({g, x_T(0)}); // x_terminal = 0 - g = casadi::MX::vertcat({g, x_T(1)}); // y_terminal = 0 - g = casadi::MX::vertcat({g, x_T(2)}); // z_terminal = 0 - g = casadi::MX::vertcat({g, x_T(3)}); // vx_terminal = 0 - g = casadi::MX::vertcat({g, x_T(4)}); // vy_terminal = 0 - g = casadi::MX::vertcat({g, x_T(5)}); // vz_terminal = 0 - - // Control constraints (HCW specific) - // ux, uy, uz are bounded by u_max and u_min (this is defined later) - // Also, the thrust magnitude is bounded by u_max_norm and u_min_norm - for (int t = 0; t < horizon; t++) - { - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - g = casadi::MX::vertcat({g, casadi::MX::norm_2(u_t) - u_max_norm}); - g = casadi::MX::vertcat({g, casadi::MX::norm_2(u_t) - u_min_norm}); - } - - // NLP - casadi::MXDict nlp = { - {"x", z}, - {"f", J}, - {"g", g}}; - - // Solver options - casadi::Dict solver_opts; - solver_opts["ipopt.max_iter"] = 1000; - solver_opts["ipopt.print_level"] = 5; - solver_opts["print_time"] = true; - solver_opts["ipopt.tol"] = 1e-6; - - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Bounds - std::vector lbx(n_states + n_controls); - std::vector ubx(n_states + n_controls); - std::vector lbg(g.size1()); - std::vector ubg(g.size1()); - - // State bounds - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - lbx[t * state_dim + i] = -1e20; - ubx[t * state_dim + i] = 1e20; - } - } - - // Control bounds - for (int t = 0; t < horizon; t++) - { - // Acceleration bounds - lbx[n_states + t * control_dim + 0] = -u_max; - ubx[n_states + t * control_dim + 0] = u_max; - lbx[n_states + t * control_dim + 1] = -u_max; - ubx[n_states + t * control_dim + 1] = u_max; - lbx[n_states + t * control_dim + 2] = -u_max; - ubx[n_states + t * control_dim + 2] = u_max; - } - - // Constraint bounds (all equality constraints) - for (int i = 0; i < g.size1(); i++) - { - lbg[i] = 0; - ubg[i] = 0; - } - - // Initial guess - std::vector x0(n_states + n_controls, 0.0); - - // Initial state - for (int i = 0; i < state_dim; i++) - { - x0[i] = initial_state(i); - } - - // Linear interpolation for states - for (int t = 1; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - x0[t * state_dim + i] = initial_state(i) + - (goal_state(i) - initial_state(i)) * t / horizon; - } - } - - // Call the solver - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(casadi::DMDict{ - {"x0", x0}, - {"lbx", lbx}, - {"ubx", ubx}, - {"lbg", lbg}, - {"ubg", ubg}}); - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_time - start_time); - - std::cout << "Solve time: " << duration.count() << " microseconds" << std::endl; - - // Extract solution - std::vector sol = std::vector(res.at("x")); - - // Convert to state and control trajectories - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - X_sol[t](i) = sol[t * state_dim + i]; - } - } - - for (int t = 0; t < horizon; t++) - { - for (int i = 0; i < control_dim; i++) - { - U_sol[t](i) = sol[n_states + t * control_dim + i]; - } - } - - - return 0; -} \ No newline at end of file diff --git a/examples/ipopt_spacecrat_rpo.cpp b/examples/ipopt_spacecrat_rpo.cpp deleted file mode 100644 index 452b85c4..00000000 --- a/examples/ipopt_spacecrat_rpo.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ -#include -#include -#include -#include -#include -#include -#include -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -int main() -{ - // Problem parameters - int state_dim = 8; // [x y z vx vy vz mass accumulated_control_effort] - int control_dim = 3; // [ux uy uz] - int horizon = 400; - double time_horizon = 400.0; - double timestep = time_horizon / horizon; - - // HCW parameters - double mean_motion = 0.001107; - double initial_mass = 100.0; - double isp = 300.0; - double g0 = 9.80665; - double nominal_radius = 50.0; - double u_max = 10.0; - double u_min = -10.0; - double u_max_norm = 10.0; - double u_min_norm = 0.0; - - // Initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << nominal_radius, 0.0, 0.0, 0.0, -2.0*mean_motion*nominal_radius, 0.0, initial_mass, 0.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, initial_mass, 0.0; - - // Define variables - casadi::MX x = casadi::MX::sym("x", state_dim); - casadi::MX u = casadi::MX::sym("u", control_dim); - - - casadi::MX vx = x(3); - casadi::MX vy = x(4); - casadi::MX vz = x(5); - casadi::MX mass = x(6); - casadi::MX accumulated_control_effort = x(7); - casadi::MX ux = u(0); - casadi::MX uy = u(1); - casadi::MX uz = u(2); - - // Continuous dynamics (HCW) - casadi::MX dx = casadi::MX::zeros(state_dim); - - dx(0) = vx; - dx(1) = vy; - dx(2) = vz; - dx(3) = -2.0*mean_motion*vx + ux / mass; - dx(4) = -2.0*mean_motion*vy + uy / mass; - dx(5) = -2.0*mean_motion*vz + uz / mass; - dx(6) = -ux*ux - uy*uy - uz*uz / (mass*mass); - dx(7) = ux*ux + uy*uy + uz*uz; - - // Discrete dynamics using Euler integration - casadi::MX f = x + timestep * dx; - casadi::Function F("F", {x, u}, {f}); - - // Decision variables - int n_states = (horizon + 1) * state_dim; - int n_controls = horizon * control_dim; - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Objective function terms - auto running_cost = [&](casadi::MX x, casadi::MX u) - { - return 0.0; - }; - - auto terminal_cost = [&](casadi::MX x) - { - // Final cost is the terminal mass. - casadi::MX cost =-x(6); - return cost; - }; - - // Build objective (general) - casadi::MX J = 0; - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - J += running_cost(x_t, u_t); - } - J += terminal_cost(X(casadi::Slice(horizon * state_dim, (horizon + 1) * state_dim))); - - // Constraints - casadi::MX g; - - // Initial condition (general) - g = X(casadi::Slice(0, state_dim)) - - casadi::DM(std::vector(initial_state.data(), initial_state.data() + state_dim)); - - // Dynamics constraints (general) - for (int t = 0; t < horizon; t++) - { - casadi::MX x_t = X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - casadi::MX x_next = F(casadi::MXVector{x_t, u_t})[0]; - casadi::MX x_next_sym = X(casadi::Slice((t + 1) * state_dim, (t + 2) * state_dim)); - g = casadi::MX::vertcat({g, x_next_sym - x_next}); - } - - // Terminal condition (HCW specific) - // x, y, z, vx, vy, vz are zero - casadi::MX x_T = X(casadi::Slice(horizon * state_dim, (horizon + 1) * state_dim)); - g = casadi::MX::vertcat({g, x_T(0)}); // x_terminal = 0 - g = casadi::MX::vertcat({g, x_T(1)}); // y_terminal = 0 - g = casadi::MX::vertcat({g, x_T(2)}); // z_terminal = 0 - g = casadi::MX::vertcat({g, x_T(3)}); // vx_terminal = 0 - g = casadi::MX::vertcat({g, x_T(4)}); // vy_terminal = 0 - g = casadi::MX::vertcat({g, x_T(5)}); // vz_terminal = 0 - - // Control constraints (HCW specific) - // ux, uy, uz are bounded by u_max and u_min (this is defined later) - // Also, the thrust magnitude is bounded by u_max_norm and u_min_norm - for (int t = 0; t < horizon; t++) - { - casadi::MX u_t = U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - g = casadi::MX::vertcat({g, casadi::MX::norm_2(u_t) - u_max_norm}); - g = casadi::MX::vertcat({g, casadi::MX::norm_2(u_t) - u_min_norm}); - } - - // NLP - casadi::MXDict nlp = { - {"x", z}, - {"f", J}, - {"g", g}}; - - // Solver options - casadi::Dict solver_opts; - solver_opts["ipopt.max_iter"] = 1000; - solver_opts["ipopt.print_level"] = 5; - solver_opts["print_time"] = true; - solver_opts["ipopt.tol"] = 1e-6; - - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Bounds - std::vector lbx(n_states + n_controls); - std::vector ubx(n_states + n_controls); - std::vector lbg(g.size1()); - std::vector ubg(g.size1()); - - // State bounds - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - lbx[t * state_dim + i] = -1e20; - ubx[t * state_dim + i] = 1e20; - } - } - - // Control bounds - for (int t = 0; t < horizon; t++) - { - // Acceleration bounds - lbx[n_states + t * control_dim + 0] = -u_max; - ubx[n_states + t * control_dim + 0] = u_max; - lbx[n_states + t * control_dim + 1] = -u_max; - ubx[n_states + t * control_dim + 1] = u_max; - lbx[n_states + t * control_dim + 2] = -u_max; - ubx[n_states + t * control_dim + 2] = u_max; - } - - // Constraint bounds (all equality constraints) - for (int i = 0; i < g.size1(); i++) - { - lbg[i] = 0; - ubg[i] = 0; - } - - // Initial guess - std::vector x0(n_states + n_controls, 0.0); - - // Initial state - for (int i = 0; i < state_dim; i++) - { - x0[i] = initial_state(i); - } - - // Linear interpolation for states - for (int t = 1; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - x0[t * state_dim + i] = initial_state(i) + - (goal_state(i) - initial_state(i)) * t / horizon; - } - } - - // Call the solver - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(casadi::DMDict{ - {"x0", x0}, - {"lbx", lbx}, - {"ubx", ubx}, - {"lbg", lbg}, - {"ubg", ubg}}); - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_time - start_time); - - std::cout << "Solve time: " << duration.count() << " microseconds" << std::endl; - - // Extract solution - std::vector sol = std::vector(res.at("x")); - - // Convert to state and control trajectories - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - X_sol[t](i) = sol[t * state_dim + i]; - } - } - - for (int t = 0; t < horizon; t++) - { - for (int i = 0; i < control_dim; i++) - { - U_sol[t](i) = sol[n_states + t * control_dim + i]; - } - } - - - return 0; -} \ No newline at end of file diff --git a/examples/ipopt_unicycle.cpp b/examples/ipopt_unicycle.cpp deleted file mode 100644 index 0a20e171..00000000 --- a/examples/ipopt_unicycle.cpp +++ /dev/null @@ -1,459 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; -namespace fs = std::filesystem; - -int main() { - ////////// Problem Setup ////////// - const int state_dim = 3; // [x, y, theta] - const int control_dim = 2; // [v, omega] - const int horizon = 100; // Number of control intervals - const double timestep = 0.03; // Time step - - // Define initial and goal states - Eigen::VectorXd initial_state(state_dim); - initial_state << 0.0, 0.0, M_PI/4.0; - - Eigen::VectorXd goal_state(state_dim); - goal_state << 2.0, 2.0, M_PI/2.0; - // Define cost weighting matrices - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); - - Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim); - - // Terminal cost weight (optional if using terminal constraint) - Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf(0, 0) = 100.0; // x position - Qf(1, 1) = 100.0; // y position - Qf(2, 2) = 100.0; // heading - - - casadi::DM Q_dm(Q.rows(), Q.cols()); - for (int i = 0; i < Q.rows(); i++) { - for (int j = 0; j < Q.cols(); j++) { - Q_dm(i, j) = Q(i, j); - } - } - casadi::DM R_dm(R.rows(), R.cols()); - for (int i = 0; i < R.rows(); i++) { - for (int j = 0; j < R.cols(); j++) { - R_dm(i, j) = R(i, j); - } - } - casadi::DM Qf_dm(Qf.rows(), Qf.cols()); - for (int i = 0; i < Qf.rows(); i++) { - for (int j = 0; j < Qf.cols(); j++) { - Qf_dm(i, j) = Qf(i, j); - } - } - - // Define control bounds (for example, v ∈ [-1, 1] and ω ∈ [-π, π]) - Eigen::VectorXd u_min(control_dim), u_max(control_dim); - u_min << -1.0, -M_PI; - u_max << 1.0, M_PI; - - const int n_states = (horizon + 1) * state_dim; - const int n_controls = horizon * control_dim; - const int n_dec = n_states + n_controls; - - // Define symbolic variables for states and controls - casadi::MX X = casadi::MX::sym("X", n_states); - casadi::MX U = casadi::MX::sym("U", n_controls); - casadi::MX z = casadi::MX::vertcat({X, U}); - - // Helper lambdas to extract the state and control at time step t - auto X_t = [=](int t) -> casadi::MX { - return X(casadi::Slice(t * state_dim, (t + 1) * state_dim)); - }; - auto U_t = [=](int t) -> casadi::MX { - return U(casadi::Slice(t * control_dim, (t + 1) * control_dim)); - }; - - auto unicycle_dynamics = [=](casadi::MX x, casadi::MX u) -> casadi::MX { - casadi::MX x_next = casadi::MX::zeros(state_dim, 1); - casadi::MX theta = x(2); - casadi::MX v = u(0); - casadi::MX omega = u(1); - - // Use casadi's trigonometric functions - using casadi::cos; - using casadi::sin; - casadi::MX ctheta = cos(theta); - casadi::MX stheta = sin(theta); - // Euler integration discretization - x_next(0) = x(0) + v * ctheta * timestep; - x_next(1) = x(1) + v * stheta * timestep; - x_next(2) = x(2) + omega * timestep; - return x_next; - }; - - casadi::MX g; - - // Initial state constraint: X₀ = initial_state - casadi::DM init_state_dm(std::vector(initial_state.data(), initial_state.data() + state_dim)); - g = casadi::MX::vertcat({g, X_t(0) - init_state_dm}); - - // Dynamics constraints: - for (int t = 0; t < horizon; t++) { - casadi::MX x_next_expr = unicycle_dynamics(X_t(t), U_t(t)); - g = casadi::MX::vertcat({g, X_t(t + 1) - x_next_expr}); - } - - // --- Terminal Condition Constraint --- - casadi::DM goal_dm(std::vector(goal_state.data(), goal_state.data() + state_dim)); - casadi::MX terminal_constr = X_t(horizon) - goal_dm; - g = casadi::MX::vertcat({g, terminal_constr}); - - ////////// Cost Function ////////// - casadi::MX cost = casadi::MX::zeros(1, 1); - for (int t = 0; t < horizon; t++) { - casadi::MX x_diff = X_t(t) - goal_dm; - casadi::MX u_diff = U_t(t); - casadi::MX state_cost = casadi::MX::mtimes({x_diff.T(), Q_dm, x_diff}); - casadi::MX control_cost = casadi::MX::mtimes({u_diff.T(), R_dm, u_diff}); - cost = cost + state_cost + control_cost; - } - // Terminal cost - casadi::MX x_diff_final = X_t(horizon) - goal_dm; - casadi::MX terminal_cost = casadi::MX::mtimes({x_diff_final.T(), Qf_dm, x_diff_final}); - cost = cost + terminal_cost; - - ////////// Variable Bounds and Initial Guess ////////// - std::vector lbx(n_dec, -1e20); - std::vector ubx(n_dec, 1e20); - // Apply control bounds for the control segments. - for (int t = 0; t < horizon; t++) { - for (int i = 0; i < control_dim; i++) { - lbx[n_states + t * control_dim + i] = u_min(i); - ubx[n_states + t * control_dim + i] = u_max(i); - } - } - - // The complete set of constraints (g) must be equal to zero. - const int n_g = static_cast(g.size1()); - std::vector lbg(n_g, 0.0); - std::vector ubg(n_g, 0.0); - - // Provide an initial guess for the decision vector. - std::vector x0(n_dec, 0.0); - // Set the initial state portion. - for (int i = 0; i < state_dim; i++) { - x0[i] = initial_state(i); - } - // Linearly interpolate the state trajectory from initial_state to goal_state. - for (int t = 1; t <= horizon; t++) { - for (int i = 0; i < state_dim; i++) { - x0[t * state_dim + i] = initial_state(i) + (goal_state(i) - initial_state(i)) * (double)t / horizon; - } - } - // The control part of the initial guess remains zero. - - ////////// NLP Definition and IPOPT Solver Setup ////////// - std::map nlp; - nlp["x"] = z; - nlp["f"] = cost; - nlp["g"] = g; - - casadi::Dict solver_opts; - solver_opts["print_time"] = true; - solver_opts["ipopt.print_level"] = 5; - solver_opts["ipopt.max_iter"] = 500; - solver_opts["ipopt.tol"] = 1e-6; - - // Create the NLP solver instance using IPOPT. - casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); - - // Convert the initial guess and bounds into DM objects. - casadi::DM x0_dm = casadi::DM(x0); - casadi::DM lbx_dm = casadi::DM(lbx); - casadi::DM ubx_dm = casadi::DM(ubx); - casadi::DM lbg_dm = casadi::DM(lbg); - casadi::DM ubg_dm = casadi::DM(ubg); - - casadi::DMDict arg({ - {"x0", x0_dm}, - {"lbx", lbx_dm}, - {"ubx", ubx_dm}, - {"lbg", lbg_dm}, - {"ubg", ubg_dm} - }); - - ////////// Solve the NLP ////////// - auto start_time = std::chrono::high_resolution_clock::now(); - casadi::DMDict res = solver(arg); - auto end_time = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end_time - start_time; - std::cout << "Solver elapsed time: " << elapsed.count() << " s" << std::endl; - - ////////// Extract and Display the Solution ////////// - // The result 'res["x"]' is a DM vector with the optimized decision variables. - std::vector sol = std::vector(res.at("x")); - - // Convert to state and control trajectories - std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); - std::vector t_sol(horizon + 1); - for (int t = 0; t <= horizon; t++) { - t_sol[t] = t * timestep; - } - - for (int t = 0; t <= horizon; t++) - { - for (int i = 0; i < state_dim; i++) - { - X_sol[t](i) = sol[t * state_dim + i]; - } - } - - for (int t = 0; t < horizon; t++) - { - for (int i = 0; i < control_dim; i++) - { - U_sol[t](i) = sol[n_states + t * control_dim + i]; - } - } - - // Create directory for saving plot (if it doesn't exist) - const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directory(plotDirectory); - } - - // Plot the solution (x-y plane) - std::vector x_arr, y_arr, theta_arr; - for (const auto& x : X_sol) { - x_arr.push_back(x(0)); - y_arr.push_back(x(1)); - theta_arr.push_back(x(2)); - } - - // Plot the solution (control inputs) - std::vector v_arr, omega_arr; - for (const auto& u : U_sol) { - v_arr.push_back(u(0)); - omega_arr.push_back(u(1)); - } - - // ----------------------------- - // Plot states and controls - // ----------------------------- - auto f1 = figure(); - f1->size(1200, 800); - - // First subplot: Position Trajectory - auto ax1 = subplot(3, 1, 0); - auto plot_handle = plot(ax1, x_arr, y_arr, "-b"); - plot_handle->line_width(3); - title(ax1, "Position Trajectory"); - xlabel(ax1, "x [m]"); - ylabel(ax1, "y [m]"); - - // Second subplot: Heading Angle vs Time - auto ax2 = subplot(3, 1, 1); - auto heading_plot_handle = plot(ax2, t_sol, theta_arr); - heading_plot_handle->line_width(3); - title(ax2, "Heading Angle"); - xlabel(ax2, "Time [s]"); - ylabel(ax2, "theta [rad]"); - - // Fourth subplot: Control Inputs - auto ax4 = subplot(3, 1, 2); - auto p1 = plot(ax4, v_arr, "--b"); - p1->line_width(3); - p1->display_name("Acceleration"); - - hold(ax4, true); - auto p2 = plot(ax4, omega_arr, "--r"); - p2->line_width(3); - p2->display_name("Steering"); - - title(ax4, "Control Inputs"); - xlabel(ax4, "Step"); - ylabel(ax4, "Control"); - matplot::legend(ax4); - - f1->draw(); - f1->save(plotDirectory + "/unicycle_ipopt_results.png"); - - // ----------------------------- - // Animation: unicycle Trajectory - // ----------------------------- - auto f2 = figure(); - f2->size(800, 600); - auto ax_anim = f2->current_axes(); - if (!ax_anim) - { - ax_anim = axes(); - } - - double car_length = 0.35; - double car_width = 0.15; - - for (size_t i = 0; i < X_sol.size(); ++i) - { - if (i % 10 == 0) - { - ax_anim->clear(); - hold(ax_anim, true); - - double x = x_arr[i]; - double y = y_arr[i]; - double theta = theta_arr[i]; - - // Compute unicycle rectangle corners - std::vector car_x(5), car_y(5); - car_x[0] = x + car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[0] = y + car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[1] = x + car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[1] = y + car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[2] = x - car_length / 2 * cos(theta) + car_width / 2 * sin(theta); - car_y[2] = y - car_length / 2 * sin(theta) - car_width / 2 * cos(theta); - car_x[3] = x - car_length / 2 * cos(theta) - car_width / 2 * sin(theta); - car_y[3] = y - car_length / 2 * sin(theta) + car_width / 2 * cos(theta); - car_x[4] = car_x[0]; - car_y[4] = car_y[0]; - - auto car_line = plot(ax_anim, car_x, car_y); - car_line->color("black"); - car_line->line_style("solid"); - car_line->line_width(2); - car_line->display_name("Car"); - - // Plot trajectory up to current frame - std::vector traj_x(x_arr.begin(), x_arr.begin() + i + 1); - std::vector traj_y(y_arr.begin(), y_arr.begin() + i + 1); - auto traj_line = plot(ax_anim, traj_x, traj_y); - traj_line->color("blue"); - traj_line->line_style("solid"); - traj_line->line_width(1.5); - traj_line->display_name("Trajectory"); - - title(ax_anim, "unicycle Trajectory"); - xlabel(ax_anim, "x [m]"); - ylabel(ax_anim, "y [m]"); - xlim(ax_anim, {-1, 2.2}); - ylim(ax_anim, {-1, 2.2}); - // legend(ax_anim); - - std::string filename = plotDirectory + "/unicycle_frame_" + std::to_string(i) + ".png"; - f2->draw(); - f2->save(filename); - std::this_thread::sleep_for(std::chrono::milliseconds(80)); - } - } - - // ----------------------------- - // Generate GIF from frames using ImageMagick - // ----------------------------- - std::string gif_command = "convert -delay 30 " + plotDirectory + "/unicycle_frame_*.png " + plotDirectory + "/unicycle_ipopt.gif"; - std::system(gif_command.c_str()); - - std::string cleanup_command = "rm " + plotDirectory + "/unicycle_frame_*.png"; - std::system(cleanup_command.c_str()); - - - std::cout << "GIF animation created successfully: " << plotDirectory + "/unicycle_ipopt.gif" << std::endl; - - return 0; -} - -// :~/github/cddp-cpp/build$ ./examples/ipopt_unicycle - -// ****************************************************************************** -// This program contains Ipopt, a library for large-scale nonlinear optimization. -// Ipopt is released as open source code under the Eclipse Public License (EPL). -// For more information visit http://projects.coin-or.org/Ipopt -// ****************************************************************************** - -// This is Ipopt version 3.11.9, running with linear solver mumps. -// NOTE: Other linear solvers might be more efficient (see Ipopt documentation). - -// Number of nonzeros in equality constraint Jacobian...: 1106 -// Number of nonzeros in inequality constraint Jacobian.: 0 -// Number of nonzeros in Lagrangian Hessian.............: 506 - -// Total number of variables............................: 503 -// variables with only lower bounds: 0 -// variables with lower and upper bounds: 200 -// variables with only upper bounds: 0 -// Total number of equality constraints.................: 306 -// Total number of inequality constraints...............: 0 -// inequality constraints with only lower bounds: 0 -// inequality constraints with lower and upper bounds: 0 -// inequality constraints with only upper bounds: 0 - -// iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls -// 0 0.0000000e+00 2.00e-02 0.00e+00 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0 -// 1 1.3213637e+01 1.45e-02 5.18e+00 -1.0 3.57e+00 - 2.22e-01 2.77e-01h 1 -// 2 4.8004394e+01 2.56e-02 3.60e+00 -1.0 2.64e+00 0.0 2.24e-01 3.65e-01h 1 -// 3 6.2828049e+01 2.00e-02 3.08e+00 -1.0 3.97e+00 - 1.91e-01 2.19e-01h 1 -// 4 5.3377401e+01 1.25e-02 1.56e+00 -1.0 1.01e+00 - 3.34e-01 3.98e-01f 1 -// 5 5.6693670e+01 3.61e-03 6.71e-01 -1.0 1.78e+00 - 9.31e-01 7.28e-01h 1 -// 6 6.0580150e+01 2.37e-04 4.35e-02 -1.7 1.95e-01 - 9.48e-01 1.00e+00h 1 -// 7 6.0448863e+01 2.28e-05 2.20e-01 -2.5 1.36e-01 - 6.39e-01 1.00e+00f 1 -// 8 6.0165740e+01 1.35e-05 6.29e-02 -2.5 1.28e-01 - 7.42e-01 1.00e+00f 1 -// 9 6.0066412e+01 4.10e-06 5.07e-04 -2.5 5.72e-02 - 1.00e+00 1.00e+00f 1 -// iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls -// 10 5.9914306e+01 9.25e-06 2.97e-02 -3.8 5.45e-02 - 6.88e-01 1.00e+00f 1 -// 11 5.9875609e+01 1.08e-06 3.38e-03 -3.8 2.49e-02 - 9.21e-01 1.00e+00f 1 -// 12 5.9869812e+01 6.18e-08 7.97e-06 -3.8 7.32e-03 - 1.00e+00 1.00e+00h 1 -// 13 5.9857746e+01 9.70e-08 5.52e-04 -5.7 5.35e-03 - 9.49e-01 1.00e+00f 1 -// 14 5.9857287e+01 2.38e-09 5.54e-07 -5.7 2.47e-03 - 1.00e+00 1.00e+00h 1 -// 15 5.9857127e+01 3.02e-10 6.60e-08 -7.0 9.07e-04 - 1.00e+00 1.00e+00h 1 -// 16 5.9857126e+01 1.05e-12 3.89e-10 -7.0 1.04e-04 - 1.00e+00 1.00e+00h 1 - -// Number of Iterations....: 16 - -// (scaled) (unscaled) -// Objective...............: 5.9857125548898239e+01 5.9857125548898239e+01 -// Dual infeasibility......: 3.8946268432482611e-10 3.8946268432482611e-10 -// Constraint violation....: 1.0502709812953981e-12 1.0502709812953981e-12 -// Complementarity.........: 1.0259906103085439e-07 1.0259906103085439e-07 -// Overall NLP error.......: 1.0259906103085439e-07 1.0259906103085439e-07 - - -// Number of objective function evaluations = 17 -// Number of objective gradient evaluations = 17 -// Number of equality constraint evaluations = 17 -// Number of inequality constraint evaluations = 0 -// Number of equality constraint Jacobian evaluations = 17 -// Number of inequality constraint Jacobian evaluations = 0 -// Number of Lagrangian Hessian evaluations = 16 -// Total CPU secs in IPOPT (w/o function evaluations) = 0.017 -// Total CPU secs in NLP function evaluations = 0.004 - -// EXIT: Optimal Solution Found. -// solver : t_proc (avg) t_wall (avg) n_eval -// nlp_f | 242.00us ( 14.24us) 241.34us ( 14.20us) 17 -// nlp_g | 503.00us ( 29.59us) 503.62us ( 29.62us) 17 -// nlp_grad_f | 493.00us ( 27.39us) 490.74us ( 27.26us) 18 -// nlp_hess_l | 1.71ms (106.69us) 1.71ms (106.86us) 16 -// nlp_jac_g | 1.85ms (102.56us) 1.85ms (102.81us) 18 -// total | 23.72ms ( 23.72ms) 23.73ms ( 23.73ms) 1 -// Solver elapsed time: 0.0238267 s -// GIF animation created successfully: ../results/tests/unicycle_ipopt.gif \ No newline at end of file diff --git a/examples/mpc_hcw.cpp b/examples/mpc_hcw.cpp deleted file mode 100644 index 2a49e457..00000000 --- a/examples/mpc_hcw.cpp +++ /dev/null @@ -1,386 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#include -#include -#include -#include -#include - -#include "cddp.hpp" -#include "cddp_example_utils.hpp" - -#include "matplot/matplot.h" -using namespace matplot; -namespace fs = std::filesystem; -using namespace cddp; - -int main() { - // ========================================================================= - // 1) Parameters - // ========================================================================= - - // Simulation times - double tf = 600.0; // final time for simulation - int tN = 6000; // number of total time steps for simulation - double dt_sim = tf / tN; // simulation time step (0.1 s) - double Ts = 2.5; // control update period [s] - - // HCW parameters - double mean_motion = 0.001107; - double mass = 100.0; - - HCW hcw_system(dt_sim, mean_motion, mass, "euler"); // HCW system - - // MPC horizon info - double time_horizon = 400.0; // time horizon for MPC [s] - int N = 40; // predictive horizon length - double dt_mpc = time_horizon / N; // MPC time step - - // Final (reference) state - Eigen::VectorXd x_ref(6); - x_ref << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - - // Input constraints - double u_max = 1.0; // for each dimension - double u_min = -1.0; // for each dimension - - // Cost weighting - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(6,6); - { - Q(0,0) = 1e+1; Q(1,1) = 1e+1; Q(2,2) = 1e+1; - Q(3,3) = 1e-0; Q(4,4) = 1e-0; Q(5,5) = 1e-0; - } - - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(3,3); - { - R(0,0) = 1e-0; R(1,1) = 1e-0; R(2,2) = 1e-0; - } - - // Terminal cost (can be zero or nonzero). - Eigen::MatrixXd Qf = Eigen::MatrixXd::Zero(6,6); - // { - // Qf(0,0) = 1e3; Qf(1,1) = 1e3; Qf(2,2) = 1e3; - // Qf(3,3) = 1e1; Qf(4,4) = 1e1; Qf(5,5) = 1e1; - // } - - // Sample initial conditions (24). - std::vector initial_conditions; - initial_conditions.reserve(24); - auto sqrt3 = std::sqrt(3.0); - - std::vector> ics_data = { - {25.0, 25.0/sqrt3, 0, 0, 0, 0}, - {25.0, 0, 0, 0, 0, 0}, - {25.0,-25.0/sqrt3, 0, 0, 0, 0}, - {50.0, 50.0/sqrt3, 0, 0, 0, 0}, - {50.0, -0, 0, 0, 0, 0}, - {50.0,-50.0/sqrt3, 0, 0, 0, 0}, - {75.0, 75.0/sqrt3, 0, 0, 0, 0}, - {75.0, 0, 0, 0, 0, 0}, - {75.0,-75.0/sqrt3, 0, 0, 0, 0}, - {100.0,100.0/sqrt3,0, 0, 0, 0}, - {100.0,0, 0, 0, 0, 0}, - {100.0,-100.0/sqrt3,0, 0, 0, 0}, - {75.0, 25.0/sqrt3, 0, 0, 0, 0}, - {75.0,-25.0/sqrt3, 0, 0, 0, 0}, - {100.0,25.0/sqrt3, 0, 0, 0, 0}, - {100.0,-25.0/sqrt3, 0, 0, 0, 0}, - {75.0, 50.0/sqrt3, 0, 0, 0, 0}, - {75.0,-50.0/sqrt3, 0, 0, 0, 0}, - {100.0,50.0/sqrt3, 0, 0, 0, 0}, - {100.0,-50.0/sqrt3, 0, 0, 0, 0}, - {75.0, 75.0/sqrt3, 0, 0, 0, 0}, - {75.0,-75.0/sqrt3, 0, 0, 0, 0}, - {100.0,75.0/sqrt3, 0, 0, 0, 0}, - {100.0,-75.0/sqrt3, 0, 0, 0, 0} - }; - - // std::vector> ics_data = { - // {25.0/sqrt3, 25.0, 0, 0, 0, 0}, - // {0, 25.0, 0, 0, 0, 0}, - // {-25.0/sqrt3, 25.0, 0, 0, 0, 0}, - // {50.0/sqrt3, 50.0, 0, 0, 0, 0}, - // {0, 50.0, 0, 0, 0, 0}, - // {-50.0/sqrt3, 50.0, 0, 0, 0, 0}, - // {75.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {0, 75.0, 0, 0, 0, 0}, - // {-75.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {100.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {0, 100.0, 0, 0, 0, 0}, - // {-100.0/sqrt3,100.0, 0, 0, 0, 0}, - // {25.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {-25.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {25.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {-25.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {50.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {-50.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {50.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {-50.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {75.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {-75.0/sqrt3, 75.0, 0, 0, 0, 0}, - // {75.0/sqrt3, 100.0, 0, 0, 0, 0}, - // {-75.0/sqrt3, 100.0, 0, 0, 0, 0} - // }; - - for (auto &data : ics_data) { - Eigen::VectorXd x0(6); - x0 << data[0], data[1], data[2], data[3], data[4], data[5]; - initial_conditions.push_back(x0); - } - - // For plotting/recording data (x, y, z vs time). - // X_data[ i_sample ][ time_index ] = 6D state - // U_data[ i_sample ][ time_index ] = 3D control - const int num_samples = static_cast(initial_conditions.size()); - std::vector> X_data(num_samples, - std::vector(tN)); - std::vector> U_data(num_samples, - std::vector(tN - 1)); - - // ========================================================================= - // 2) Loop over each initial condition, run an MPC-like closed-loop sim - // ========================================================================= - // Optional random generator for initial guess in each MPC solve - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution d(0.0, 0.01); - - for (int i = 0; i < num_samples; ++i) { - // --------------------------------------------------------------------- - // (A) Create a new instance of the HCW system - // --------------------------------------------------------------------- - std::unique_ptr mpc_system = - std::make_unique(dt_mpc, mean_motion, mass, "euler"); - - // --------------------------------------------------------------------- - // (B) Build cost objective - // --------------------------------------------------------------------- - std::vector empty_reference; - auto objective = std::make_unique( - Q, R, Qf, x_ref, empty_reference, dt_mpc - ); - - // Adjust solver options if desired (similar to cddp_hcw.cpp) - cddp::CDDPOptions options; - options.max_iterations = 50; - options.line_search.max_iterations = 21; - options.tolerance = 1e-2; - options.verbose = false; - options.enable_parallel = false; - options.num_threads = 8; - // Regularization - options.regularization.initial_value = 1e-5; - - // --------------------------------------------------------------------- - // (C) Setup CDDP solver - // --------------------------------------------------------------------- - cddp::CDDP cddp_solver(/*initial_state=*/initial_conditions[i], - /*goal_state=*/x_ref, - /*horizon=*/N, - /*timestep=*/dt_mpc, - /*system=*/std::move(mpc_system), - /*objective=*/std::move(objective), - /*options=*/options); - - // Control box constraints - Eigen::VectorXd u_lower(3), u_upper(3); - u_lower << u_min, u_min, u_min; - u_upper << u_max, u_max, u_max; - cddp_solver.addPathConstraint( - "ControlConstraint", - std::make_unique(u_lower, u_upper) - ); - - // --------------------------------------------------------------------- - // (D) Initialize storage for trajectory - // --------------------------------------------------------------------- - for (int k = 0; k < tN; ++k) { - X_data[i][k] = Eigen::VectorXd::Zero(6); - } - for (int k = 0; k < tN - 1; ++k) { - U_data[i][k] = Eigen::VectorXd::Zero(3); - } - X_data[i][0] = initial_conditions[i]; - - // --------------------------------------------------------------------- - // (E) Main time loop for simulation - // --------------------------------------------------------------------- - Eigen::VectorXd current_state = initial_conditions[i]; - Eigen::VectorXd current_u(3); - current_u.setZero(); - - std::vector X_init(N + 1, Eigen::VectorXd::Zero(6)); - std::vector U_init(N, Eigen::VectorXd::Zero(3)); - - X_init[0] = initial_conditions[i]; - - for (auto& u : U_init) { - u << d(gen), d(gen), d(gen); - // u << 0.0, 0.0, 0.0; // zero thruster - } - - // // Propagate the initial guess through the dynamics - for (int k = 0; k < N; ++k) { - X_init[k + 1] = hcw_system.getDiscreteDynamics(X_init[k], U_init[k], k * dt_mpc); - } - - // Assign them to the solver - cddp_solver.setInitialTrajectory(X_init, U_init); - - for (int k = 0; k < tN - 1; ++k) { - double t = k * dt_sim; - - // Check if the state is close to the goal - if (current_state.norm() < 1e-2) { - X_data[i][k + 1] = current_state; - current_u.setZero(); - U_data[i][k] = current_u; - continue; - } - - // Re-solve MPC every Ts seconds fmod - if (fmod(t, Ts) < dt_sim) { - // Update the solver’s current-state as the new “initial state” - cddp_solver.setInitialState(current_state); - - // Solve the MPC problem with horizon N - cddp::CDDPSolution sol = cddp_solver.solve(); - - // Extract the *first* control from that horizon - const auto& control_traj = sol.control_trajectory; - current_u = control_traj[0]; - - const auto& X_sequence = sol.state_trajectory; - const auto& U_sequence = sol.control_trajectory; - - cddp_solver.setInitialTrajectory(X_sequence, U_sequence); - } - - // ----------------------------------------------------------------- - // Advance the state by one simulation step dt_sim - // ----------------------------------------------------------------- - { - current_state = hcw_system.getDiscreteDynamics(current_state, current_u, k * dt_sim); - } - - // Save data - X_data[i][k + 1] = current_state; - U_data[i][k] = current_u; - } // end for k - } // end for i over samples - - // ========================================================================= - // 3) Plot the X-Y trajectories for all samples - // ========================================================================= - { - auto fig = figure(); - fig->size(800, 600); - title("HCW MPC Trajectories (x vs y)"); - for (int i = 0; i < num_samples; ++i) { - std::vector xvals, yvals; - xvals.reserve(tN); - yvals.reserve(tN); - for (int k = 0; k < tN; ++k) { - xvals.push_back(X_data[i][k](0)); // rx - yvals.push_back(X_data[i][k](1)); // ry - } - plot(xvals, yvals); - } - - // Plot the initial conditions - for (int i = 0; i < num_samples; ++i) { - std::vector xvals, yvals; - xvals.push_back(initial_conditions[i](0)); - yvals.push_back(initial_conditions[i](1)); - plot(xvals, yvals, "ro"); - } - - xlabel("x [m]"); - ylabel("y [m]"); - // axis limit - xlim({-10, 110}); - ylim({-100, 100}); - - // xlim(-100, 100); - // ylim(-10, 110); - grid(true); - - // Create results directory - const std::string plotDirectory = "../results/simulations"; - cddp::example::ensurePlotDir(plotDirectory); - std::string figPath = plotDirectory + "/hcw_mpc_cddp_xaxis_trajectories.png"; - save(figPath); - // show(); - } - - // Plot control - { - figure()->size(800, 600); - title("HCW MPC Control Inputs"); - for (int i = 0; i < num_samples; ++i) { - std::vector time_vals; - std::vector u1_vals, u2_vals, u3_vals, u_ub, u_lb; - time_vals.reserve(tN - 1); - u1_vals.reserve(tN - 1); - u2_vals.reserve(tN - 1); - u3_vals.reserve(tN - 1); - u_ub.reserve(tN - 1); - u_lb.reserve(tN - 1); - for (int k = 0; k < tN - 1; ++k) { - time_vals.push_back(k * dt_sim); - u1_vals.push_back(U_data[i][k](0)); - u2_vals.push_back(U_data[i][k](1)); - u3_vals.push_back(U_data[i][k](2)); - u_ub.push_back(u_max); - u_lb.push_back(u_min); - } - subplot(3, 1, 1); - plot(time_vals, u1_vals); - plot(time_vals, u_ub, "r--"); - plot(time_vals, u_lb, "r--"); - xlabel("Time [s]"); - ylabel("u1"); - grid(true); - - subplot(3, 1, 2); - plot(time_vals, u2_vals); - plot(time_vals, u_ub, "r--"); - plot(time_vals, u_lb, "r--"); - xlabel("Time [s]"); - ylabel("u2"); - grid(true); - - subplot(3, 1, 3); - plot(time_vals, u3_vals); - plot(time_vals, u_ub, "r--"); - plot(time_vals, u_lb, "r--"); - xlabel("Time [s]"); - ylabel("u3"); - grid(true); - } - // Create results directory - const std::string plotDirectory = "../results/simulations"; - cddp::example::ensurePlotDir(plotDirectory); - std::string figPath = plotDirectory + "/hcw_mpc_cddp_controls.png"; - save(figPath); - // show(); - } - - - std::cout << "MPC simulation for HCW completed successfully!\n"; - return 0; -} diff --git a/examples/python_portfolio.py b/examples/python_portfolio.py new file mode 100644 index 00000000..dd03a221 --- /dev/null +++ b/examples/python_portfolio.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +"""Generate animated Python portfolio demos for the pycddp bindings.""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") + +import python_portfolio_lib as portfolio + + +def _positive_int(value: str) -> int: + parsed = int(value) + if parsed < 1: + raise argparse.ArgumentTypeError("value must be >= 1") + return parsed + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--demo", + choices=["all", *sorted(portfolio.DEMO_BUILDERS)], + default="all", + help="Which demo to generate.", + ) + parser.add_argument( + "--output-dir", + type=Path, + default=Path("docs/assets/python_portfolio"), + help="Directory where GIFs will be written.", + ) + parser.add_argument( + "--fps", + type=_positive_int, + default=16, + help="Animation frame rate.", + ) + parser.add_argument( + "--dpi", + type=_positive_int, + default=110, + help="Output DPI for the generated GIFs.", + ) + parser.add_argument( + "--frame-step", + type=_positive_int, + default=2, + help="Use every Nth solver state as an animation frame.", + ) + return parser.parse_args() + + +def main() -> None: + args = parse_args() + demo_names = ( + sorted(portfolio.DEMO_BUILDERS) + if args.demo == "all" + else [args.demo] + ) + + args.output_dir.mkdir(parents=True, exist_ok=True) + + for name in demo_names: + result = portfolio.build_demo(name) + output_path = args.output_dir / f"{result.slug}.gif" + portfolio.save_animation( + result, + output_path, + fps=args.fps, + dpi=args.dpi, + frame_step=args.frame_step, + ) + print( + f"{result.title}: {output_path} " + f"(solver={result.solver_name}, final_error={result.final_error:.4f})" + ) + + +if __name__ == "__main__": + main() diff --git a/examples/python_portfolio_lib.py b/examples/python_portfolio_lib.py new file mode 100644 index 00000000..2b5cbc1a --- /dev/null +++ b/examples/python_portfolio_lib.py @@ -0,0 +1,1426 @@ +"""Portfolio-quality Python demos and animations for pycddp.""" + +from __future__ import annotations + +import csv +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any, Callable + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib import animation +from matplotlib.patches import Circle, FancyArrowPatch, Rectangle +from matplotlib.transforms import Affine2D + +import pycddp + + +_BACKGROUND = "#f5efe3" +_PANEL = "#fffaf2" +_TEXT = "#1d2533" +_GRID = "#d8cdb8" +_ACCENT = "#1f6f8b" +_SECONDARY = "#d97706" +_TERTIARY = "#9c2f2f" +_SUCCESS = "#1f7a5c" +_MUTED = "#8d99a6" + + +@dataclass(slots=True) +class DemoResult: + slug: str + title: str + solver_name: str + solution: pycddp.CDDPSolution + states: np.ndarray + controls: np.ndarray + time_points: np.ndarray + target_state: np.ndarray + metadata: dict[str, Any] = field(default_factory=dict) + + @property + def final_error(self) -> float: + if "final_error_override" in self.metadata: + return float(self.metadata["final_error_override"]) + return float(np.linalg.norm(self.states[-1] - self.target_state)) + + +@dataclass(slots=True) +class TrackReference: + x: float + y: float + s: float + heading: float + curvature: float + v_ref: float + tangent: np.ndarray + normal: np.ndarray + + +@dataclass(slots=True) +class TrackData: + x: np.ndarray + y: np.ndarray + s: np.ndarray + heading: np.ndarray + curvature: np.ndarray + v_ref: np.ndarray + width: float + length: float + _s_ext: np.ndarray = field(init=False, repr=False) + _x_ext: np.ndarray = field(init=False, repr=False) + _y_ext: np.ndarray = field(init=False, repr=False) + _heading_ext: np.ndarray = field(init=False, repr=False) + _curvature_ext: np.ndarray = field(init=False, repr=False) + _v_ref_ext: np.ndarray = field(init=False, repr=False) + _inner_boundary: np.ndarray = field(init=False, repr=False) + _outer_boundary: np.ndarray = field(init=False, repr=False) + + def __post_init__(self) -> None: + heading_unwrapped = np.unwrap(np.asarray(self.heading, dtype=float)) + x = np.asarray(self.x, dtype=float) + y = np.asarray(self.y, dtype=float) + s = np.asarray(self.s, dtype=float) + curvature = np.asarray(self.curvature, dtype=float) + v_ref = np.asarray(self.v_ref, dtype=float) + tangent = np.column_stack((np.cos(heading_unwrapped), np.sin(heading_unwrapped))) + normal = np.column_stack((-tangent[:, 1], tangent[:, 0])) + + object.__setattr__(self, "_s_ext", np.concatenate([s, [self.length]])) + object.__setattr__(self, "_x_ext", np.concatenate([x, [x[0]]])) + object.__setattr__(self, "_y_ext", np.concatenate([y, [y[0]]])) + object.__setattr__( + self, + "_heading_ext", + np.concatenate([heading_unwrapped, [heading_unwrapped[0] + 2.0 * np.pi]]), + ) + object.__setattr__(self, "_curvature_ext", np.concatenate([curvature, [curvature[0]]])) + object.__setattr__(self, "_v_ref_ext", np.concatenate([v_ref, [v_ref[0]]])) + object.__setattr__(self, "_inner_boundary", np.column_stack((x - self.width * normal[:, 0], y - self.width * normal[:, 1]))) + object.__setattr__(self, "_outer_boundary", np.column_stack((x + self.width * normal[:, 0], y + self.width * normal[:, 1]))) + + @property + def inner_boundary(self) -> np.ndarray: + return self._inner_boundary + + @property + def outer_boundary(self) -> np.ndarray: + return self._outer_boundary + + def wrap_progress(self, progress: float) -> float: + return float(np.mod(progress, self.length)) + + def interpolate(self, progress: float) -> TrackReference: + wrapped = self.wrap_progress(progress) + x = float(np.interp(wrapped, self._s_ext, self._x_ext)) + y = float(np.interp(wrapped, self._s_ext, self._y_ext)) + heading = float(np.interp(wrapped, self._s_ext, self._heading_ext)) + curvature = float(np.interp(wrapped, self._s_ext, self._curvature_ext)) + v_ref = float(np.interp(wrapped, self._s_ext, self._v_ref_ext)) + tangent = np.array([np.cos(heading), np.sin(heading)], dtype=float) + normal = np.array([-np.sin(heading), np.cos(heading)], dtype=float) + return TrackReference( + x=x, + y=y, + s=wrapped, + heading=float(_wrap_angle(heading)), + curvature=curvature, + v_ref=v_ref, + tangent=tangent, + normal=normal, + ) + + def reference_state(self, progress: float) -> np.ndarray: + ref = self.interpolate(progress) + return np.array([ref.x, ref.y, ref.heading, progress], dtype=float) + + +def _solution_arrays(solution: pycddp.CDDPSolution) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + states = np.vstack([np.asarray(x, dtype=float) for x in solution.state_trajectory]) + if solution.control_trajectory: + controls = np.vstack([np.asarray(u, dtype=float) for u in solution.control_trajectory]) + else: + controls = np.zeros((0, 0), dtype=float) + time_points = np.asarray(solution.time_points, dtype=float) + return states, controls, time_points + + +def _ensure_solution_has_trajectory( + solution: pycddp.CDDPSolution, + label: str, + *, + require_controls: bool = True, +) -> None: + if not solution.state_trajectory: + raise RuntimeError(f"{label} returned an empty state trajectory.") + if require_controls and not solution.control_trajectory: + raise RuntimeError(f"{label} returned an empty control trajectory.") + + +def _rollout_system( + model: pycddp.DynamicalSystem, + initial_state: np.ndarray, + controls: list[np.ndarray], +) -> tuple[list[np.ndarray], list[np.ndarray]]: + state = np.asarray(initial_state, dtype=float).copy() + states = [state.copy()] + rolled_controls: list[np.ndarray] = [] + for control in controls: + control_array = np.asarray(control, dtype=float).copy() + rolled_controls.append(control_array) + state = np.asarray(model.get_discrete_dynamics(state, control_array), dtype=float) + states.append(state.copy()) + return states, rolled_controls + + +def _wrap_angle(angle: float) -> float: + return float((angle + np.pi) % (2.0 * np.pi) - np.pi) + + +def _build_racing_track(num_points: int = 420, width: float = 1.35) -> TrackData: + t = np.linspace(0.0, 2.0 * np.pi, num_points, endpoint=False) + x = 7.6 * np.cos(t) + 2.0 * np.cos(2.0 * t) - 0.55 * np.cos(3.0 * t) + y = 4.8 * np.sin(t) - 1.35 * np.sin(2.0 * t) + 0.25 * np.sin(3.0 * t) + + dx = np.roll(x, -1) - x + dy = np.roll(y, -1) - y + ds = np.hypot(dx, dy) + length = float(np.sum(ds)) + s = np.concatenate([[0.0], np.cumsum(ds[:-1])]) + + heading = np.unwrap(np.arctan2(dy, dx)) + curvature = np.gradient(heading, s, edge_order=2) + v_ref = np.clip(4.2 / (1.0 + 5.0 * np.abs(curvature)), 1.8, 4.2) + + return TrackData( + x=x, + y=y, + s=s, + heading=heading, + curvature=curvature, + v_ref=v_ref, + width=width, + length=length, + ) + + +def _portfolio_track_path() -> Path: + return Path(__file__).resolve().parent / "data" / "mpcc_racing_track.csv" + + +def _load_track_csv( + path: str | Path, + width: float = 0.18, + coordinate_scale: float = 1.0, +) -> TrackData: + with Path(path).open() as handle: + rows = [{key: float(value) for key, value in row.items()} for row in csv.DictReader(handle)] + + x = coordinate_scale * np.asarray([row["x"] for row in rows], dtype=float) + y = coordinate_scale * np.asarray([row["y"] for row in rows], dtype=float) + dx = np.roll(x, -1) - x + dy = np.roll(y, -1) - y + ds = np.hypot(dx, dy) + length = float(np.sum(ds)) + s = np.concatenate([[0.0], np.cumsum(ds[:-1])]) + + heading = np.unwrap(np.arctan2(dy, dx)) + curvature = np.gradient(heading, s, edge_order=2) + abs_curvature = np.abs(curvature) + v_ref = np.clip(np.sqrt(1.35 / np.maximum(abs_curvature, 0.12)), 1.0, 2.2) + v_ref = np.minimum(v_ref, np.roll(v_ref, -1) + 0.18) + + return TrackData( + x=x, + y=y, + s=s, + heading=heading, + curvature=curvature, + v_ref=v_ref, + width=width, + length=length, + ) + + +class MpccBicycle(pycddp.DynamicalSystem): + def __init__(self, timestep: float, wheelbase: float = 0.33) -> None: + super().__init__(4, 3, timestep, "euler") + self.wheelbase = float(wheelbase) + + def get_continuous_dynamics( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> np.ndarray: + speed, steering_angle, progress_rate = np.asarray(control, dtype=float) + heading = float(state[2]) + beta = float(np.arctan(0.5 * np.tan(steering_angle))) + return np.array( + [ + speed * np.cos(heading + beta), + speed * np.sin(heading + beta), + 2.0 * speed * np.sin(beta) / self.wheelbase, + progress_rate, + ], + dtype=float, + ) + + def get_state_jacobian( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> np.ndarray: + speed = float(control[0]) + heading = float(state[2]) + steering_angle = float(control[1]) + beta = float(np.arctan(0.5 * np.tan(steering_angle))) + return np.array( + [ + [0.0, 0.0, -speed * np.sin(heading + beta), 0.0], + [0.0, 0.0, speed * np.cos(heading + beta), 0.0], + [0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0], + ], + dtype=float, + ) + + def get_control_jacobian( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> np.ndarray: + heading = float(state[2]) + speed = float(control[0]) + steering_angle = float(control[1]) + tan_delta = np.tan(steering_angle) + sec_sq = 1.0 / (np.cos(steering_angle) ** 2) + beta = float(np.arctan(0.5 * tan_delta)) + beta_prime = 0.5 * sec_sq / (1.0 + 0.25 * tan_delta * tan_delta) + return np.array( + [ + [np.cos(heading + beta), -speed * np.sin(heading + beta) * beta_prime, 0.0], + [np.sin(heading + beta), speed * np.cos(heading + beta) * beta_prime, 0.0], + [ + 2.0 * np.sin(beta) / self.wheelbase, + 2.0 * speed * np.cos(beta) * beta_prime / self.wheelbase, + 0.0, + ], + [0.0, 0.0, 1.0], + ], + dtype=float, + ) + + def get_state_hessian( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> list[np.ndarray]: + return [np.zeros((4, 4), dtype=float) for _ in range(4)] + + def get_control_hessian( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> list[np.ndarray]: + return [np.zeros((3, 3), dtype=float) for _ in range(4)] + + def get_cross_hessian( + self, + state: np.ndarray, + control: np.ndarray, + time: float = 0.0, + ) -> list[np.ndarray]: + return [np.zeros((3, 4), dtype=float) for _ in range(4)] + + +class ContouringObjective(pycddp.NonlinearObjective): + def __init__(self, timestep: float, track: TrackData) -> None: + super().__init__(timestep) + self.dt = float(timestep) + self.track = track + self.w_contour = 220.0 + self.w_lag = 28.0 + self.w_heading = 8.0 + self.w_progress = 4.5 + self.w_sync = 10.0 + self.w_steer = 0.25 + self.w_boundary = 320.0 + self.w_terminal = 6.0 + + def tracking_terms( + self, + state: np.ndarray, + ) -> tuple[float, float, float, TrackReference]: + ref = self.track.interpolate(float(state[3])) + delta_position = np.asarray(state[:2], dtype=float) - np.array([ref.x, ref.y]) + contour_error = float(ref.normal @ delta_position) + lag_error = float(ref.tangent @ delta_position) + heading_error = _wrap_angle(float(state[2]) - ref.heading) + return contour_error, lag_error, heading_error, ref + + def running_cost( + self, + state: np.ndarray, + control: np.ndarray, + index: int, + ) -> float: + contour_error, lag_error, heading_error, ref = self.tracking_terms(state) + speed, steering_angle, progress_rate = np.asarray(control, dtype=float) + boundary_error = max(0.0, abs(contour_error) - 0.72 * self.track.width) + progress_error = progress_rate - ref.v_ref + sync_error = speed - progress_rate + return float( + self.dt + * ( + self.w_contour * contour_error**2 + + self.w_lag * lag_error**2 + + self.w_heading * heading_error**2 + + self.w_progress * progress_error**2 + + self.w_sync * sync_error**2 + + self.w_steer * steering_angle**2 + + self.w_boundary * boundary_error**2 + - 1.6 * progress_rate + ) + ) + + def terminal_cost(self, final_state: np.ndarray) -> float: + contour_error, lag_error, heading_error, _ = self.tracking_terms(final_state) + boundary_error = max(0.0, abs(contour_error) - 0.75 * self.track.width) + return float( + self.w_terminal + * ( + 220.0 * contour_error**2 + + 42.0 * lag_error**2 + + 18.0 * heading_error**2 + + 180.0 * boundary_error**2 + ) + ) + + +def _default_options(max_iterations: int = 120) -> pycddp.CDDPOptions: + opts = pycddp.CDDPOptions() + opts.max_iterations = max_iterations + opts.verbose = False + opts.print_solver_header = False + opts.return_iteration_info = True + return opts + + +def _make_result( + slug: str, + title: str, + target_state: np.ndarray, + solution: pycddp.CDDPSolution, + states: np.ndarray | None = None, + controls: np.ndarray | None = None, + time_points: np.ndarray | None = None, + solver_name: str | None = None, + **metadata: Any, +) -> DemoResult: + if states is None or controls is None or time_points is None: + states, controls, time_points = _solution_arrays(solution) + return DemoResult( + slug=slug, + title=title, + solver_name=solver_name or solution.solver_name, + solution=solution, + states=states, + controls=controls, + time_points=time_points, + target_state=np.asarray(target_state, dtype=float), + metadata=metadata, + ) + + +def solve_pendulum_demo() -> DemoResult: + dt = 0.05 + horizon = 120 + x0 = np.array([0.0, 0.0], dtype=float) + xref = np.array([np.pi, 0.0], dtype=float) + + opts = _default_options(max_iterations=150) + opts.tolerance = 1e-5 + opts.acceptable_tolerance = 1e-4 + opts.regularization.initial_value = 1e-6 + + model = pycddp.Pendulum(dt, length=0.5, mass=1.0, damping=0.01) + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(model) + solver.set_objective( + pycddp.QuadraticObjective( + 0.1 * np.eye(2), + 0.02 * np.eye(1), + 200.0 * np.eye(2), + xref, + [], + dt, + ) + ) + solver.add_constraint( + "control_limits", + pycddp.ControlConstraint(np.array([-30.0]), np.array([30.0])), + ) + seed_controls = [ + np.array([8.0], dtype=float) if index < 25 else np.zeros(1, dtype=float) + for index in range(horizon) + ] + seed_states, seed_controls = _rollout_system(model, x0, seed_controls) + solver.set_initial_trajectory(seed_states, seed_controls) + + solution = solver.solve(pycddp.SolverType.CLDDP) + _ensure_solution_has_trajectory(solution, "Pendulum portfolio solve") + return _make_result( + slug="pendulum_swing_up", + title="Pendulum Swing-Up", + target_state=xref, + solution=solution, + timestep=dt, + length=0.5, + subtitle="Seeded CLDDP swing-up from the hanging equilibrium", + ) + + +def solve_cartpole_demo() -> DemoResult: + dt = 0.05 + horizon = 100 + x0 = np.array([0.0, 0.0, 0.0, 0.0], dtype=float) + xref = np.array([0.0, np.pi, 0.0, 0.0], dtype=float) + + opts = _default_options(max_iterations=120) + opts.tolerance = 1e-6 + opts.acceptable_tolerance = 1e-5 + opts.regularization.initial_value = 1e-5 + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(pycddp.CartPole(dt)) + solver.set_objective( + pycddp.QuadraticObjective( + np.zeros((4, 4)), + 0.1 * np.eye(1), + 80.0 * np.eye(4), + xref, + [], + dt, + ) + ) + solver.add_constraint( + "force_limits", + pycddp.ControlConstraint(np.array([-5.0]), np.array([5.0])), + ) + solver.set_initial_trajectory( + [x0.copy() for _ in range(horizon + 1)], + [np.zeros(1, dtype=float) for _ in range(horizon)], + ) + + solution = solver.solve(pycddp.SolverType.CLDDP) + _ensure_solution_has_trajectory(solution, "Cart-pole portfolio solve") + return _make_result( + slug="cartpole_swing_up", + title="Cart-Pole Swing-Up", + target_state=xref, + solution=solution, + timestep=dt, + pole_length=0.5, + subtitle="Control-limited CLDDP solve", + ) + + +def solve_unicycle_demo() -> DemoResult: + dt = 0.03 + horizon = 100 + x0 = np.array([0.0, 0.0, np.pi / 4.0], dtype=float) + xref = np.array([2.0, 2.0, np.pi / 2.0], dtype=float) + obstacle_center = np.array([1.0, 1.0], dtype=float) + obstacle_radius = 0.4 + + opts = _default_options(max_iterations=100) + opts.tolerance = 1e-4 + + solver_objective = pycddp.QuadraticObjective( + np.zeros((3, 3)), + 0.05 * np.eye(2), + np.diag([100.0, 100.0, 50.0]), + xref, + [], + dt, + ) + control_limits = pycddp.ControlConstraint( + np.array([-1.1, -np.pi]), + np.array([1.1, np.pi]), + ) + initial_states = [x0.copy() for _ in range(horizon + 1)] + initial_controls = [np.zeros(2, dtype=float) for _ in range(horizon)] + + # Seed the constrained solve with the smoother unconstrained route so the + # obstacle-avoidance case is stable across repeated runs. + baseline = pycddp.CDDP(x0, xref, horizon, dt, opts) + baseline.set_dynamical_system(pycddp.Unicycle(dt)) + baseline.set_objective(solver_objective) + baseline.add_constraint("control_limits", control_limits) + baseline.set_initial_trajectory(initial_states, initial_controls) + baseline_solution = baseline.solve(pycddp.SolverType.CLDDP) + _ensure_solution_has_trajectory( + baseline_solution, + "Unicycle baseline portfolio solve", + ) + + best_solution: pycddp.CDDPSolution | None = None + best_score: tuple[float, float, float] | None = None + + for _ in range(4): + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(pycddp.Unicycle(dt)) + solver.set_objective( + pycddp.QuadraticObjective( + np.zeros((3, 3)), + 0.05 * np.eye(2), + np.diag([100.0, 100.0, 50.0]), + xref, + [], + dt, + ) + ) + solver.add_constraint( + "control_limits", + pycddp.ControlConstraint( + np.array([-1.1, -np.pi]), + np.array([1.1, np.pi]), + ), + ) + solver.add_constraint( + "obstacle", + pycddp.BallConstraint(obstacle_radius, obstacle_center), + ) + solver.set_initial_trajectory( + list(baseline_solution.state_trajectory), + list(baseline_solution.control_trajectory), + ) + + candidate = solver.solve(pycddp.SolverType.IPDDP) + if not candidate.state_trajectory or not candidate.control_trajectory: + continue + candidate_error = float( + np.linalg.norm(np.asarray(candidate.state_trajectory[-1]) - xref) + ) + candidate_score = ( + float(candidate.final_primal_infeasibility), + candidate_error, + float(candidate.final_objective), + ) + if best_score is None or candidate_score < best_score: + best_score = candidate_score + best_solution = candidate + if candidate.final_primal_infeasibility < 1e-3 and candidate_error < 0.02: + break + + if best_solution is None: + raise RuntimeError("Unicycle portfolio solve did not produce a valid candidate trajectory.") + return _make_result( + slug="unicycle_obstacle_avoidance", + title="Unicycle Obstacle Avoidance", + target_state=xref, + solution=best_solution, + timestep=dt, + obstacle_center=obstacle_center, + obstacle_radius=obstacle_radius, + subtitle="IPDDP with a ball constraint", + ) + + +def _reference_seed_controls(track: TrackData, start_progress: float, horizon: int, dt: float, wheelbase: float) -> list[np.ndarray]: + controls: list[np.ndarray] = [] + progress = float(start_progress) + for _ in range(horizon): + ref = track.interpolate(progress) + steering = float(np.clip(np.arctan(wheelbase * ref.curvature), -0.42, 0.42)) + speed = float(np.clip(ref.v_ref, 1.0, 2.2)) + progress_rate = float(np.clip(speed + 0.12, 1.0, 2.4)) + controls.append(np.array([speed, steering, progress_rate], dtype=float)) + progress = progress + dt * progress_rate + return controls + + +def _rollout_mpcc_seed( + model: MpccBicycle, + track: TrackData, + initial_state: np.ndarray, + controls: list[np.ndarray], +) -> tuple[list[np.ndarray], list[np.ndarray]]: + state = np.asarray(initial_state, dtype=float).copy() + states = [state.copy()] + rolled_controls: list[np.ndarray] = [] + for control in controls: + control_array = np.asarray(control, dtype=float).copy() + rolled_controls.append(control_array) + state = np.asarray(model.get_discrete_dynamics(state, control_array), dtype=float) + states.append(state.copy()) + return states, rolled_controls + + +def solve_mpcc_demo(simulation_steps: int | None = None, horizon: int = 16) -> DemoResult: + dt = 0.10 + wheelbase = 0.20 + track = _load_track_csv(_portfolio_track_path()) + if simulation_steps is None: + nominal_progress = max(float(np.mean(track.v_ref)) + 0.15, 1.55) + simulation_steps = int(np.ceil(1.08 * track.length / (dt * nominal_progress))) + model = MpccBicycle(dt, wheelbase=wheelbase) + objective = ContouringObjective(dt, track) + + opts = _default_options(max_iterations=12) + opts.use_ilqr = True + opts.enable_parallel = False + opts.return_iteration_info = False + opts.tolerance = 2e-3 + opts.acceptable_tolerance = 6e-3 + opts.regularization.initial_value = 1e-5 + opts.line_search.max_iterations = 9 + + start_ref = track.interpolate(0.0) + initial_state = np.array( + [ + start_ref.x + 0.28 * track.width * start_ref.normal[0], + start_ref.y + 0.28 * track.width * start_ref.normal[1], + start_ref.heading - 0.08, + 0.0, + ], + dtype=float, + ) + + solver = pycddp.CDDP( + initial_state, + track.reference_state(0.0), + horizon, + dt, + opts, + ) + solver.set_dynamical_system(model) + solver.set_objective(objective) + solver.add_constraint( + "control_limits", + pycddp.ControlConstraint( + np.array([0.8, -0.55, 0.8], dtype=float), + np.array([2.4, 0.55, 2.6], dtype=float), + ), + ) + + seed_controls = _reference_seed_controls(track, initial_state[3], horizon, dt, wheelbase) + seed_states, seed_controls = _rollout_mpcc_seed(model, track, initial_state, seed_controls) + + state = initial_state.copy() + executed_states = [state.copy()] + contour_errors: list[float] = [] + lag_errors: list[float] = [] + heading_errors: list[float] = [] + solve_times_ms: list[float] = [] + executed_controls: list[np.ndarray] = [] + predicted_paths: list[np.ndarray] = [] + reference_windows: list[np.ndarray] = [] + last_solution: pycddp.CDDPSolution | None = None + + for _ in range(simulation_steps): + contour_error, lag_error, heading_error, _ = objective.tracking_terms(state) + contour_errors.append(contour_error) + lag_errors.append(lag_error) + heading_errors.append(heading_error) + + solver.set_initial_state(state) + solver.set_reference_state(track.reference_state(state[3])) + solver.set_initial_trajectory(seed_states, seed_controls) + solution = solver.solve(pycddp.SolverType.CLDDP) + _ensure_solution_has_trajectory(solution, "MPCC portfolio solve") + last_solution = solution + + predicted_states, predicted_controls, _ = _solution_arrays(solution) + if predicted_controls.size == 0: + raise RuntimeError("MPCC demo produced an empty control trajectory.") + + predicted_paths.append(predicted_states[:, :2].copy()) + reference_windows.append( + np.vstack([track.reference_state(progress)[0:2] for progress in predicted_states[:, 3]]) + ) + solve_times_ms.append(float(solution.solve_time_ms)) + + control = predicted_controls[0].copy() + executed_controls.append(control) + state = np.asarray(model.get_discrete_dynamics(state, control), dtype=float) + state[2] = _wrap_angle(state[2]) + executed_states.append(state.copy()) + + shifted_controls = [u.copy() for u in predicted_controls[1:]] + if shifted_controls: + shifted_controls.append(shifted_controls[-1].copy()) + else: + shifted_controls = _reference_seed_controls(track, state[3], horizon, dt, wheelbase) + if len(shifted_controls) < horizon: + fallback = _reference_seed_controls(track, state[3], horizon - len(shifted_controls), dt, wheelbase) + shifted_controls.extend(fallback) + seed_states, seed_controls = _rollout_mpcc_seed(model, track, state, shifted_controls[:horizon]) + + final_contour, final_lag, final_heading, final_ref = objective.tracking_terms(executed_states[-1]) + contour_errors.append(final_contour) + lag_errors.append(final_lag) + heading_errors.append(final_heading) + + if last_solution is None: + raise RuntimeError("MPCC demo did not produce any MPC solves.") + + closed_loop_states = np.vstack(executed_states) + closed_loop_controls = np.vstack(executed_controls) + time_points = np.arange(len(executed_states), dtype=float) * dt + + return _make_result( + slug="mpcc_racing_line", + title="MPCC Racing Line", + target_state=np.array([final_ref.x, final_ref.y, final_ref.heading, final_ref.s], dtype=float), + solution=last_solution, + states=closed_loop_states, + controls=closed_loop_controls, + time_points=time_points, + solver_name=f"{last_solution.solver_name} MPC", + timestep=dt, + track=track, + track_width=track.width, + contour_errors=np.asarray(contour_errors, dtype=float), + lag_errors=np.asarray(lag_errors, dtype=float), + heading_errors=np.asarray(heading_errors, dtype=float), + solve_times_ms=np.asarray(solve_times_ms, dtype=float), + predicted_paths=predicted_paths, + reference_windows=reference_windows, + final_error_override=float(np.hypot(final_contour, final_lag)), + subtitle="Full-lap receding-horizon contouring control on a closed circuit", + ) + + +DEMO_BUILDERS: dict[str, Callable[[], DemoResult]] = { + "pendulum": solve_pendulum_demo, + "cartpole": solve_cartpole_demo, + "unicycle": solve_unicycle_demo, + "mpcc": solve_mpcc_demo, +} + + +def build_demo(name: str) -> DemoResult: + try: + return DEMO_BUILDERS[name]() + except KeyError as exc: + choices = ", ".join(sorted(DEMO_BUILDERS)) + raise ValueError(f"Unknown demo '{name}'. Expected one of: {choices}.") from exc + + +def _style_axes(ax: plt.Axes) -> None: + ax.set_facecolor(_PANEL) + for spine in ax.spines.values(): + spine.set_color(_GRID) + spine.set_linewidth(1.0) + ax.tick_params(colors=_TEXT, labelsize=9) + ax.grid(True, color=_GRID, alpha=0.55, linewidth=0.7) + + +def _style_scene_axes(ax: plt.Axes) -> None: + ax.set_facecolor(_PANEL) + for spine in ax.spines.values(): + spine.set_visible(False) + ax.set_xticks([]) + ax.set_yticks([]) + ax.grid(False) + + +def _metric_bounds(values: np.ndarray, minimum: float = 1.0) -> tuple[float, float]: + amplitude = max(float(np.max(np.abs(values))) * 1.15, minimum) + return -amplitude, amplitude + + +def _apply_title(fig: plt.Figure, result: DemoResult) -> None: + fig.suptitle(result.title, fontsize=18, fontweight="bold", color=_TEXT, y=0.965) + fig.subplots_adjust(top=0.81) + + +def save_animation( + result: DemoResult, + output_path: str | Path, + fps: int = 16, + dpi: int = 110, + frame_step: int = 2, +) -> Path: + if fps < 1: + raise ValueError(f"fps must be >= 1, got {fps}.") + if dpi < 1: + raise ValueError(f"dpi must be >= 1, got {dpi}.") + if frame_step < 1: + raise ValueError(f"frame_step must be >= 1, got {frame_step}.") + + output = Path(output_path) + output.parent.mkdir(parents=True, exist_ok=True) + + if result.slug == "pendulum_swing_up": + fig, anim = _animate_pendulum(result, fps=fps, frame_step=frame_step) + elif result.slug == "cartpole_swing_up": + fig, anim = _animate_cartpole(result, fps=fps, frame_step=frame_step) + elif result.slug == "unicycle_obstacle_avoidance": + fig, anim = _animate_unicycle(result, fps=fps, frame_step=frame_step) + elif result.slug == "mpcc_racing_line": + fig, anim = _animate_mpcc(result, fps=fps, frame_step=frame_step) + else: + raise ValueError(f"No animation renderer for demo '{result.slug}'.") + + writer = animation.PillowWriter(fps=fps) + anim.save(output, writer=writer, dpi=dpi) + plt.close(fig) + return output + + +def _animate_pendulum( + result: DemoResult, + fps: int, + frame_step: int, +) -> tuple[plt.Figure, animation.FuncAnimation]: + states = result.states + controls = result.controls[:, 0] + time_points = result.time_points + pendulum_length = float(result.metadata["length"]) + frames = list(range(0, len(states), frame_step)) + if frames[-1] != len(states) - 1: + frames.append(len(states) - 1) + + fig = plt.figure(figsize=(9.2, 5.2), facecolor=_BACKGROUND) + gs = fig.add_gridspec( + 2, + 2, + width_ratios=[1.55, 1.0], + height_ratios=[1.0, 1.0], + wspace=0.16, + hspace=0.20, + ) + ax_main = fig.add_subplot(gs[:, 0]) + ax_phase = fig.add_subplot(gs[0, 1]) + ax_ctrl = fig.add_subplot(gs[1, 1]) + _apply_title(fig, result) + _style_scene_axes(ax_main) + _style_axes(ax_phase) + _style_axes(ax_ctrl) + + limit = pendulum_length * 1.45 + ax_main.set_xlim(-limit, limit) + ax_main.set_ylim(-limit, limit) + ax_main.set_aspect("equal") + ax_main.set_title("Swing-Up Motion", color=_TEXT, fontsize=11, pad=10) + orbit = Circle((0.0, 0.0), pendulum_length, facecolor="none", edgecolor=_GRID, linewidth=1.0, linestyle="--") + ax_main.add_patch(orbit) + ax_main.scatter([0.0], [0.0], s=35, color=_TEXT, zorder=4) + target_x = pendulum_length * np.sin(result.target_state[0]) + target_y = -pendulum_length * np.cos(result.target_state[0]) + ghost_rod, = ax_main.plot([0.0, target_x], [0.0, target_y], color=_MUTED, linewidth=2.0, linestyle="--") + ghost_bob = Circle((target_x, target_y), 0.06, facecolor="none", edgecolor=_MUTED, linewidth=1.6, linestyle="--") + ax_main.add_patch(ghost_bob) + + ax_phase.set_title("Phase Portrait", color=_TEXT, fontsize=11, pad=10) + ax_phase.set_xlabel("Angle [rad]", color=_TEXT) + ax_phase.set_ylabel("Angular rate [rad/s]", color=_TEXT) + theta_min = float(np.min(states[:, 0])) + theta_max = float(np.max(states[:, 0])) + omega_min = float(np.min(states[:, 1])) + omega_max = float(np.max(states[:, 1])) + theta_pad = max(0.2, 0.08 * max(abs(theta_min), abs(theta_max), 1.0)) + omega_pad = max(0.2, 0.12 * max(abs(omega_min), abs(omega_max), 1.0)) + ax_phase.set_xlim(theta_min - theta_pad, theta_max + theta_pad) + ax_phase.set_ylim(omega_min - omega_pad, omega_max + omega_pad) + ax_phase.axvline(result.target_state[0], color=_MUTED, linewidth=1.0, linestyle="--") + ax_phase.axhline(0.0, color=_GRID, linewidth=1.0) + + ax_ctrl.set_xlim(time_points[0], time_points[-2] if len(time_points) > 1 else 1.0) + ctrl_min, ctrl_max = _metric_bounds(controls) + ax_ctrl.set_ylim(ctrl_min, ctrl_max) + ax_ctrl.set_title("Control Torque", color=_TEXT, fontsize=11, pad=10) + ax_ctrl.set_xlabel("Time [s]", color=_TEXT) + ax_ctrl.set_ylabel("Torque", color=_TEXT) + + bob_trail, = ax_main.plot([], [], color=_SECONDARY, linewidth=2.2, alpha=0.7) + rod, = ax_main.plot([], [], color=_ACCENT, linewidth=3.2) + bob = Circle((0.0, 0.0), 0.07, color=_TERTIARY) + ax_main.add_patch(bob) + torque_text = ax_main.text( + 0.03, + 0.95, + "", + transform=ax_main.transAxes, + ha="left", + va="top", + fontsize=10, + color=_TEXT, + bbox={"boxstyle": "round,pad=0.35", "facecolor": "#fff7ea", "edgecolor": _GRID}, + ) + + phase_path, = ax_phase.plot(states[:, 0], states[:, 1], color=_GRID, linewidth=1.8) + phase_trace, = ax_phase.plot([], [], color=_ACCENT, linewidth=2.2) + phase_marker, = ax_phase.plot([], [], "o", color=_TERTIARY, markersize=7) + + control_history, = ax_ctrl.plot(time_points[:-1], controls, color=_ACCENT, linewidth=2.0) + control_marker, = ax_ctrl.plot([], [], "o", color=_SECONDARY, markersize=7) + + def update(frame_index: int) -> tuple[Any, ...]: + theta = states[frame_index, 0] + x = pendulum_length * np.sin(theta) + y = -pendulum_length * np.cos(theta) + trail_start = max(0, frame_index - 30) + trail_x = pendulum_length * np.sin(states[trail_start : frame_index + 1, 0]) + trail_y = -pendulum_length * np.cos(states[trail_start : frame_index + 1, 0]) + bob_trail.set_data(trail_x, trail_y) + rod.set_data([0.0, x], [0.0, y]) + bob.center = (x, y) + phase_trace.set_data(states[: frame_index + 1, 0], states[: frame_index + 1, 1]) + phase_marker.set_data([states[frame_index, 0]], [states[frame_index, 1]]) + control_idx = min(frame_index, len(controls) - 1) + control_marker.set_data([time_points[control_idx]], [controls[control_idx]]) + torque_text.set_text( + f"angle = {theta:+.2f} rad\nrate = {states[frame_index, 1]:+.2f} rad/s\ntorque = {controls[control_idx]:+.2f}" + ) + return ( + orbit, + ghost_rod, + ghost_bob, + bob_trail, + rod, + bob, + phase_path, + phase_trace, + phase_marker, + control_history, + control_marker, + torque_text, + ) + + return fig, animation.FuncAnimation( + fig, + update, + frames=frames, + interval=1000 / fps, + blit=False, + ) + + +def _animate_cartpole( + result: DemoResult, + fps: int, + frame_step: int, +) -> tuple[plt.Figure, animation.FuncAnimation]: + states = result.states + controls = result.controls[:, 0] + time_points = result.time_points + pole_length = float(result.metadata["pole_length"]) + frames = list(range(0, len(states), frame_step)) + if frames[-1] != len(states) - 1: + frames.append(len(states) - 1) + + fig = plt.figure(figsize=(8.8, 5.0), facecolor=_BACKGROUND) + gs = fig.add_gridspec(1, 2, width_ratios=[1.45, 1.0], wspace=0.18) + ax_main = fig.add_subplot(gs[0, 0]) + ax_force = fig.add_subplot(gs[0, 1]) + _apply_title(fig, result) + _style_scene_axes(ax_main) + _style_axes(ax_force) + + ax_main.set_xlim(-2.0, 2.0) + ax_main.set_ylim(-0.9, 1.2) + ax_main.set_title("Swing-Up Motion", color=_TEXT, fontsize=11, pad=10) + ax_main.axhline(0.0, color=_GRID, linewidth=2.0) + ghost_cart = Rectangle((-0.18, -0.1), 0.36, 0.2, facecolor="none", edgecolor=_MUTED, linewidth=1.4, linestyle="--") + ax_main.add_patch(ghost_cart) + ghost_pole, = ax_main.plot([0.0, 0.0], [0.1, -pole_length + 0.1], color=_MUTED, linewidth=2.0, linestyle="--") + + ax_force.set_xlim(time_points[0], time_points[-2] if len(time_points) > 1 else 1.0) + force_min, force_max = _metric_bounds(controls) + ax_force.set_ylim(force_min, force_max) + ax_force.set_title("Control Force", color=_TEXT, fontsize=11, pad=10) + ax_force.set_xlabel("Time [s]", color=_TEXT) + ax_force.set_ylabel("Force", color=_TEXT) + + cart = Rectangle((-0.18, -0.1), 0.36, 0.2, facecolor=_ACCENT, edgecolor=_TEXT, linewidth=1.2) + ax_main.add_patch(cart) + pole, = ax_main.plot([], [], color=_TERTIARY, linewidth=3.0) + bob = Circle((0.0, 0.0), 0.05, color=_SECONDARY) + ax_main.add_patch(bob) + path_line, = ax_main.plot([], [], color=_SUCCESS, linewidth=1.6, alpha=0.8) + state_text = ax_main.text( + 0.03, + 0.95, + "", + transform=ax_main.transAxes, + ha="left", + va="top", + fontsize=10, + color=_TEXT, + bbox={"boxstyle": "round,pad=0.35", "facecolor": "#fff7ea", "edgecolor": _GRID}, + ) + + force_line, = ax_force.plot(time_points[:-1], controls, color=_ACCENT, linewidth=2.0) + force_marker, = ax_force.plot([], [], "o", color=_SECONDARY, markersize=7) + + def update(frame_index: int) -> tuple[Any, ...]: + cart_x = states[frame_index, 0] + theta = states[frame_index, 1] + cart.set_x(cart_x - cart.get_width() / 2.0) + pivot = np.array([cart_x, cart.get_y() + cart.get_height() / 2.0]) + pole_tip = pivot + np.array([pole_length * np.sin(theta), -pole_length * np.cos(theta)]) + pole.set_data([pivot[0], pole_tip[0]], [pivot[1], pole_tip[1]]) + bob.center = tuple(pole_tip) + path_line.set_data(states[: frame_index + 1, 0], np.zeros(frame_index + 1)) + force_idx = min(frame_index, len(controls) - 1) + force_marker.set_data([time_points[force_idx]], [controls[force_idx]]) + state_text.set_text( + f"x = {cart_x:+.2f} m\nangle = {theta:+.2f} rad" + ) + return ( + ghost_cart, + ghost_pole, + cart, + pole, + bob, + path_line, + force_line, + force_marker, + state_text, + ) + + return fig, animation.FuncAnimation( + fig, + update, + frames=frames, + interval=1000 / fps, + blit=False, + ) + + +def _animate_unicycle( + result: DemoResult, + fps: int, + frame_step: int, +) -> tuple[plt.Figure, animation.FuncAnimation]: + states = result.states + controls = result.controls + time_points = result.time_points + obstacle_center = np.asarray(result.metadata["obstacle_center"], dtype=float) + obstacle_radius = float(result.metadata["obstacle_radius"]) + frames = list(range(0, len(states), frame_step)) + if frames[-1] != len(states) - 1: + frames.append(len(states) - 1) + + fig = plt.figure(figsize=(9.2, 5.2), facecolor=_BACKGROUND) + gs = fig.add_gridspec( + 2, + 2, + width_ratios=[1.45, 1.0], + height_ratios=[1.0, 1.0], + wspace=0.16, + hspace=0.20, + ) + ax_map = fig.add_subplot(gs[:, 0]) + ax_speed = fig.add_subplot(gs[0, 1]) + ax_turn = fig.add_subplot(gs[1, 1]) + _apply_title(fig, result) + _style_axes(ax_map) + _style_axes(ax_speed) + _style_axes(ax_turn) + + ax_map.set_xlim(-0.35, 2.35) + ax_map.set_ylim(-0.35, 2.35) + ax_map.set_aspect("equal") + ax_map.set_title("Trajectory", color=_TEXT, fontsize=11, pad=10) + ax_map.set_xlabel("x [m]", color=_TEXT) + ax_map.set_ylabel("y [m]", color=_TEXT) + ax_map.set_xticks([0.0, 1.0, 2.0]) + ax_map.set_yticks([0.0, 1.0, 2.0]) + + obstacle = Circle(obstacle_center, obstacle_radius, facecolor="#fbd38d", edgecolor=_SECONDARY, linewidth=2.0, alpha=0.55) + ax_map.add_patch(obstacle) + ax_map.scatter(states[0, 0], states[0, 1], color=_SUCCESS, s=55) + ax_map.scatter(result.target_state[0], result.target_state[1], color=_TERTIARY, s=55) + ax_map.annotate("start", (states[0, 0], states[0, 1]), xytext=(6, -14), textcoords="offset points", color=_SUCCESS, fontsize=9) + ax_map.annotate("goal", (result.target_state[0], result.target_state[1]), xytext=(6, 6), textcoords="offset points", color=_TERTIARY, fontsize=9) + full_path, = ax_map.plot(states[:, 0], states[:, 1], color=_GRID, linewidth=1.8, linestyle="--") + trail, = ax_map.plot([], [], color=_ACCENT, linewidth=2.4) + vehicle = Rectangle((-0.12, -0.05), 0.24, 0.10, facecolor=_ACCENT, edgecolor=_TEXT, linewidth=1.2) + ax_map.add_patch(vehicle) + heading = FancyArrowPatch((0.0, 0.0), (0.0, 0.0), arrowstyle="-|>", mutation_scale=12, color=_SECONDARY, linewidth=2.0) + ax_map.add_patch(heading) + + control_speed = controls[:, 0] + control_turn = controls[:, 1] + ax_speed.set_xlim(time_points[0], time_points[-2] if len(time_points) > 1 else 1.0) + speed_min, speed_max = _metric_bounds(control_speed) + ax_speed.set_ylim(speed_min, speed_max) + ax_speed.set_title("Linear Speed", color=_TEXT, fontsize=10, pad=8) + ax_speed.set_ylabel("m/s", color=_TEXT) + speed_line, = ax_speed.plot(time_points[:-1], control_speed, color=_ACCENT, linewidth=2.0) + speed_marker, = ax_speed.plot([], [], "o", color=_ACCENT, markersize=7) + + ax_turn.set_xlim(time_points[0], time_points[-2] if len(time_points) > 1 else 1.0) + turn_min, turn_max = _metric_bounds(control_turn) + ax_turn.set_ylim(turn_min, turn_max) + ax_turn.set_title("Turn Rate", color=_TEXT, fontsize=10, pad=8) + ax_turn.set_xlabel("Time [s]", color=_TEXT) + ax_turn.set_ylabel("rad/s", color=_TEXT) + turn_line, = ax_turn.plot(time_points[:-1], control_turn, color=_SECONDARY, linewidth=2.0) + turn_marker, = ax_turn.plot([], [], "o", color=_SECONDARY, markersize=7) + + status_text = ax_map.text( + 0.03, + 0.95, + "", + transform=ax_map.transAxes, + ha="left", + va="top", + fontsize=10, + color=_TEXT, + bbox={"boxstyle": "round,pad=0.35", "facecolor": "#fff7ea", "edgecolor": _GRID}, + ) + + def update(frame_index: int) -> tuple[Any, ...]: + x, y, theta = states[frame_index] + trail.set_data(states[: frame_index + 1, 0], states[: frame_index + 1, 1]) + vehicle.set_xy((x - vehicle.get_width() / 2.0, y - vehicle.get_height() / 2.0)) + vehicle.set_transform( + Affine2D().rotate_around(x, y, theta) + ax_map.transData + ) + arrow_length = 0.25 + heading.set_positions((x, y), (x + arrow_length * np.cos(theta), y + arrow_length * np.sin(theta))) + ctrl_idx = min(frame_index, len(control_speed) - 1) + speed_marker.set_data([time_points[ctrl_idx]], [control_speed[ctrl_idx]]) + turn_marker.set_data([time_points[ctrl_idx]], [control_turn[ctrl_idx]]) + obstacle_margin = np.min( + np.linalg.norm(states[: frame_index + 1, :2] - obstacle_center, axis=1) + ) - obstacle_radius + status_text.set_text( + f"heading = {theta:+.2f} rad\nclearance = {obstacle_margin:+.2f} m" + ) + return ( + obstacle, + full_path, + trail, + vehicle, + heading, + speed_line, + speed_marker, + turn_line, + turn_marker, + status_text, + ) + + return fig, animation.FuncAnimation( + fig, + update, + frames=frames, + interval=1000 / fps, + blit=False, + ) + + +def _animate_mpcc( + result: DemoResult, + fps: int, + frame_step: int, +) -> tuple[plt.Figure, animation.FuncAnimation]: + states = result.states + controls = result.controls + time_points = result.time_points + track = result.metadata["track"] + contour_errors = np.asarray(result.metadata["contour_errors"], dtype=float) + lag_errors = np.asarray(result.metadata["lag_errors"], dtype=float) + solve_times_ms = np.asarray(result.metadata["solve_times_ms"], dtype=float) + predicted_paths = result.metadata["predicted_paths"] + reference_windows = result.metadata["reference_windows"] + frames = list(range(0, len(states), frame_step)) + if frames[-1] != len(states) - 1: + frames.append(len(states) - 1) + + fig = plt.figure(figsize=(10.4, 5.8), facecolor=_BACKGROUND) + gs = fig.add_gridspec( + 2, + 2, + width_ratios=[1.7, 1.0], + height_ratios=[1.0, 1.0], + wspace=0.18, + hspace=0.22, + ) + ax_map = fig.add_subplot(gs[:, 0]) + ax_error = fig.add_subplot(gs[0, 1]) + ax_control = fig.add_subplot(gs[1, 1]) + _apply_title(fig, result) + _style_axes(ax_map) + _style_axes(ax_error) + _style_axes(ax_control) + + map_margin = track.width + 0.95 + ax_map.set_xlim(float(np.min(track.x) - map_margin), float(np.max(track.x) + map_margin)) + ax_map.set_ylim(float(np.min(track.y) - map_margin), float(np.max(track.y) + map_margin)) + ax_map.set_aspect("equal") + ax_map.set_title("Closed-Loop Racing Line", color=_TEXT, fontsize=11, pad=10) + ax_map.set_xlabel("x [m]", color=_TEXT) + ax_map.set_ylabel("y [m]", color=_TEXT) + + closed_centerline = np.vstack([np.column_stack((track.x, track.y)), [track.x[0], track.y[0]]]) + closed_inner = np.vstack([track.inner_boundary, track.inner_boundary[0]]) + closed_outer = np.vstack([track.outer_boundary, track.outer_boundary[0]]) + ax_map.plot(closed_outer[:, 0], closed_outer[:, 1], color=_GRID, linewidth=1.3) + ax_map.plot(closed_inner[:, 0], closed_inner[:, 1], color=_GRID, linewidth=1.3) + ax_map.plot(closed_centerline[:, 0], closed_centerline[:, 1], color=_MUTED, linewidth=1.1, linestyle="--") + + start_ref = track.interpolate(0.0) + ax_map.scatter([start_ref.x], [start_ref.y], color=_SUCCESS, s=54, zorder=5) + ax_map.annotate( + "start", + (start_ref.x, start_ref.y), + xytext=(6, -14), + textcoords="offset points", + color=_SUCCESS, + fontsize=9, + ) + + reference_preview, = ax_map.plot([], [], color=_SECONDARY, linewidth=2.0, alpha=0.9) + prediction_preview, = ax_map.plot([], [], color=_TERTIARY, linewidth=2.0, alpha=0.9) + driven_path, = ax_map.plot([], [], color=_ACCENT, linewidth=2.8) + vehicle_marker, = ax_map.plot([], [], marker="o", color=_ACCENT, markersize=8) + heading_arrow = FancyArrowPatch( + (0.0, 0.0), + (0.0, 0.0), + arrowstyle="-|>", + mutation_scale=14, + color=_ACCENT, + linewidth=2.2, + ) + ax_map.add_patch(heading_arrow) + + metric_times = time_points + metric_end = metric_times[-1] if len(metric_times) > 1 else 1.0 + ax_error.set_xlim(metric_times[0], metric_end) + combined_error = np.concatenate([contour_errors, lag_errors]) + error_min, error_max = _metric_bounds(combined_error, minimum=0.4) + ax_error.set_ylim(error_min, error_max) + ax_error.set_title("Tracking Error", color=_TEXT, fontsize=11, pad=10) + ax_error.set_ylabel("meters", color=_TEXT) + contour_line, = ax_error.plot(metric_times, contour_errors, color=_ACCENT, linewidth=2.0, label="contour") + lag_line, = ax_error.plot(metric_times, lag_errors, color=_SECONDARY, linewidth=2.0, label="lag") + contour_marker, = ax_error.plot([], [], "o", color=_ACCENT, markersize=6) + lag_marker, = ax_error.plot([], [], "o", color=_SECONDARY, markersize=6) + legend = ax_error.legend( + loc="upper right", + frameon=True, + facecolor="#fff7ea", + edgecolor=_GRID, + fontsize=8, + ) + for text in legend.get_texts(): + text.set_color(_TEXT) + + control_times = time_points[:-1] + control_end = control_times[-1] if len(control_times) > 0 else 1.0 + ax_control.set_xlim(time_points[0], control_end) + combined_controls = np.concatenate([controls[:, 0], controls[:, 1], solve_times_ms / 40.0]) + control_min, control_max = _metric_bounds(combined_controls, minimum=0.8) + ax_control.set_ylim(control_min, control_max) + ax_control.set_title("Command Profile", color=_TEXT, fontsize=11, pad=10) + ax_control.set_xlabel("Time [s]", color=_TEXT) + ax_control.set_ylabel("scaled units", color=_TEXT) + speed_line, = ax_control.plot(control_times, controls[:, 0], color=_SUCCESS, linewidth=2.0, label="speed") + steer_line, = ax_control.plot(control_times, controls[:, 1], color=_TERTIARY, linewidth=2.0, label="steer") + solve_line, = ax_control.plot( + control_times, + solve_times_ms / 40.0, + color=_MUTED, + linewidth=1.8, + linestyle="--", + label="solve time / 40", + ) + speed_marker, = ax_control.plot([], [], "o", color=_SUCCESS, markersize=6) + steer_marker, = ax_control.plot([], [], "o", color=_TERTIARY, markersize=6) + solve_marker, = ax_control.plot([], [], "o", color=_MUTED, markersize=6) + control_legend = ax_control.legend( + loc="upper right", + frameon=True, + facecolor="#fff7ea", + edgecolor=_GRID, + fontsize=8, + ) + for text in control_legend.get_texts(): + text.set_color(_TEXT) + + status_text = ax_map.text( + 0.03, + 0.96, + "", + transform=ax_map.transAxes, + ha="left", + va="top", + fontsize=10, + color=_TEXT, + bbox={"boxstyle": "round,pad=0.35", "facecolor": "#fff7ea", "edgecolor": _GRID}, + ) + + def update(frame_index: int) -> tuple[Any, ...]: + driven_path.set_data(states[: frame_index + 1, 0], states[: frame_index + 1, 1]) + x, y, heading, progress = states[frame_index] + vehicle_marker.set_data([x], [y]) + arrow_length = 0.85 + heading_arrow.set_positions( + (x, y), + (x + arrow_length * np.cos(heading), y + arrow_length * np.sin(heading)), + ) + + preview_index = min(frame_index, len(predicted_paths) - 1) + reference_preview.set_data( + reference_windows[preview_index][:, 0], + reference_windows[preview_index][:, 1], + ) + prediction_preview.set_data( + predicted_paths[preview_index][:, 0], + predicted_paths[preview_index][:, 1], + ) + + contour_marker.set_data([metric_times[frame_index]], [contour_errors[frame_index]]) + lag_marker.set_data([metric_times[frame_index]], [lag_errors[frame_index]]) + + control_index = min(frame_index, len(controls) - 1) + speed_marker.set_data([control_times[control_index]], [controls[control_index, 0]]) + steer_marker.set_data([control_times[control_index]], [controls[control_index, 1]]) + solve_marker.set_data([control_times[control_index]], [solve_times_ms[control_index] / 40.0]) + + speed = controls[control_index, 0] + lap_fraction = progress / track.length + status_text.set_text( + f"lap = {lap_fraction:4.2f}x\n" + f"progress = {progress:5.1f} m\n" + f"speed = {speed:4.2f} m/s\n" + f"contour = {contour_errors[frame_index]:+4.2f} m" + ) + return ( + reference_preview, + prediction_preview, + driven_path, + vehicle_marker, + heading_arrow, + contour_line, + lag_line, + contour_marker, + lag_marker, + speed_line, + steer_line, + solve_line, + speed_marker, + steer_marker, + solve_marker, + status_text, + ) + + return fig, animation.FuncAnimation( + fig, + update, + frames=frames, + interval=1000 / fps, + blit=False, + ) diff --git a/examples/test_barrier_strategies.cpp b/examples/test_barrier_strategies.cpp deleted file mode 100644 index 6ca19e3e..00000000 --- a/examples/test_barrier_strategies.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/* - Example demonstrating the three barrier update strategies for IPDDP and MSIPDDP -*/ - -#include -#include -#include -#include - -#include "cddp.hpp" - -int main() -{ - // Problem setup - const int horizon = 50; - const double timestep = 0.05; - const int state_dim = 3; - const int control_dim = 2; - - // Initial and goal states - Eigen::VectorXd X0(state_dim); - X0 << 0.0, 0.0, 0.0; // x, y, theta - - Eigen::VectorXd Xg(state_dim); - Xg << 2.0, 2.0, 0.0; - - // Test each barrier strategy - std::vector solvers = {"IPDDP", "MSIPDDP"}; - std::vector strategies = { - cddp::BarrierStrategy::ADAPTIVE, - cddp::BarrierStrategy::MONOTONIC, - cddp::BarrierStrategy::IPOPT - }; - std::vector strategy_names = {"ADAPTIVE", "MONOTONIC", "IPOPT"}; - - for (const auto& solver_name : solvers) - { - std::cout << "\n========================================\n"; - std::cout << "Testing " << solver_name << " Solver\n"; - std::cout << "========================================\n"; - - for (size_t i = 0; i < strategies.size(); ++i) - { - std::cout << "\n--- Barrier Strategy: " << strategy_names[i] << " ---\n"; - - // Create CDDP solver first - cddp::CDDP cddp_solver(X0, Xg, horizon, timestep); - - // Create and set dynamics model - auto dynamics = std::make_unique(timestep); - cddp_solver.setDynamicalSystem(std::move(dynamics)); - - // Create and set objective - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(state_dim, state_dim); - Q(0, 0) = 10.0; // x - Q(1, 1) = 10.0; // y - Q(2, 2) = 1.0; // theta - - Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim); - R(0, 0) = 0.1; // linear velocity - R(1, 1) = 0.1; // angular velocity - - Eigen::MatrixXd Qf = 100.0 * Q; - - std::vector empty_reference; - auto objective = std::make_unique( - Q, R, Qf, Xg, empty_reference, timestep); - cddp_solver.setObjective(std::move(objective)); - - // Add constraints - Eigen::VectorXd u_upper(control_dim); - u_upper << 1.0, 2.0; // max linear vel, max angular vel - - cddp_solver.addPathConstraint("ControlConstraint", - std::make_unique(-u_upper, u_upper)); - - // State bounds (keep robot in a region) - Eigen::VectorXd x_lower(state_dim), x_upper_state(state_dim); - x_lower << -0.5, -0.5, -M_PI; - x_upper_state << 2.5, 2.5, M_PI; - - cddp_solver.addPathConstraint("StateConstraint", - std::make_unique(x_lower, x_upper_state)); - - // Configure options - cddp::CDDPOptions opts; - opts.max_iterations = 100; - opts.tolerance = 1e-4; - opts.verbose = false; - opts.debug = true; // Enable debug output to see barrier updates - opts.return_iteration_info = true; - - // Set barrier strategy - if (solver_name == "IPDDP") { - opts.ipddp.barrier.strategy = strategies[i]; - opts.ipddp.barrier.mu_initial = 1.0; - opts.ipddp.barrier.mu_update_factor = 0.2; - opts.ipddp.barrier.mu_update_power = 1.5; - } else { - opts.msipddp.barrier.strategy = strategies[i]; - opts.msipddp.barrier.mu_initial = 1.0; - opts.msipddp.barrier.mu_update_factor = 0.2; - opts.msipddp.barrier.mu_update_power = 1.5; - } - - cddp_solver.setOptions(opts); - - // Initialize with straight line trajectory - std::vector X_init(horizon + 1); - std::vector U_init(horizon); - - for (int t = 0; t <= horizon; ++t) { - double alpha = static_cast(t) / horizon; - X_init[t] = (1.0 - alpha) * X0 + alpha * Xg; - } - - for (int t = 0; t < horizon; ++t) { - U_init[t] = Eigen::VectorXd::Zero(control_dim); - } - - cddp_solver.setInitialTrajectory(X_init, U_init); - - // Solve - cddp::CDDPSolution solution = cddp_solver.solve(solver_name); - - // Print results - const auto& status_message = solution.status_message; - auto iterations = solution.iterations_completed; - auto solve_time = solution.solve_time_ms; - auto final_cost = solution.final_objective; - - std::cout << "Status: " << status_message << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Final cost: " << final_cost << "\n"; - std::cout << "Solve time: " << solve_time << " ms\n"; - - // Extract final barrier parameter - std::cout << "Final barrier μ: " << solution.final_barrier_mu << "\n"; - } - } - - std::cout << "\n========================================\n"; - std::cout << "Test completed successfully!\n"; - std::cout << "========================================\n"; - - return 0; -} \ No newline at end of file diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index 27e6fccf..e1b2cf5d 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -51,7 +51,6 @@ #include "dynamics_model/spacecraft_landing2d.hpp" #include "dynamics_model/lti_system.hpp" #include "dynamics_model/dreyfus_rocket.hpp" -#include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/usv_3dof.hpp" #include "dynamics_model/euler_attitude.hpp" #include "dynamics_model/quaternion_attitude.hpp" diff --git a/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp b/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp deleted file mode 100644 index 81893b26..00000000 --- a/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp +++ /dev/null @@ -1,188 +0,0 @@ -/* - Copyright 2024 - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -#ifndef CDDP_SPACECRAFT_ROE_HPP -#define CDDP_SPACECRAFT_ROE_HPP - -#include "cddp_core/dynamical_system.hpp" -#include - -namespace cddp -{ - - /** - * @brief Spacecraft Relative Orbital Elements (ROE) Dynamics - * - * State vector (6 dimensions): - * x = [ da, dlambda, dex, dey, dix, diy] - * where: - * - da : Relative semi-major axis - * - dlambda : Relative mean longitude (or some variant of relative angle) - * - dex : Relative x-component of eccentricity vector - * - dey : Relative y-component of eccentricity vector - * - dix : Relative x-component of inclination vector - * - diy : Relative y-component of inclination vector - * - M : Mean anomaly [rad] - * - * Control vector (3 dimensions): - * u = [ ur, ut, un ] - * where: - * - ur : radial acceleration [m/s^2] - * - ut : tangential acceleration [m/s^2] - * - un : normal acceleration [m/s^2] - */ - class SpacecraftROE : public DynamicalSystem - { - public: - /** - * @brief Constructor - * @param timestep Integration timestep [s] - * @param integration_type Integration method (e.g. "rk4") - * @param a Semi-major axis of the reference orbit [m] - * @param u0 Initial mean argument of latitude [rad] - */ - SpacecraftROE( - double timestep, - const std::string &integration_type, - double a, - double u0 = 0.0, - double mass_kg = 1.0); - - // State indices - static constexpr int STATE_DA = 0; ///< Relative semi-major axis - static constexpr int STATE_DLAMBDA = 1; ///< Relative mean longitude - static constexpr int STATE_DEX = 2; ///< Relative eccentricity x-component - static constexpr int STATE_DEY = 3; ///< Relative eccentricity y-component - static constexpr int STATE_DIX = 4; ///< Relative inclination x-component - static constexpr int STATE_DIY = 5; ///< Relative inclination y-component - static constexpr int STATE_DIM = 6; ///< State dimension - - // Control indices - static constexpr int CONTROL_UR = 0; ///< Radial acceleration - static constexpr int CONTROL_UT = 1; ///< Tangential acceleration - static constexpr int CONTROL_UN = 2; ///< Normal acceleration - static constexpr int CONTROL_DIM = 3; ///< Control dimension - - /** - * @brief Compute continuous-time dynamics in ROE coordinates - * @param state Current ROE state vector - * @param control Current control vector [ur, ut, un] - * @param time Current time - * @return Derivative of the ROE state vector - */ - Eigen::VectorXd getContinuousDynamics( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const override; - - /** - * @brief Compute continuous-time dynamics in ROE coordinates using autodiff - * @param state Current ROE state vector - * @param control Current control vector [ur, ut, un] - * @param time Current time - * @return Derivative of the ROE state vector - */ - VectorXdual2nd getContinuousDynamicsAutodiff( - const VectorXdual2nd &state, - const VectorXdual2nd &control, double time) const override; - - /** - * @brief Compute state Jacobian matrix via numerical finite difference - * @param state Current ROE state vector - * @param control Current control vector - * @param time Current time - * @return State Jacobian matrix (d f / d state) - */ - Eigen::MatrixXd getStateJacobian( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const override; - - /** - * @brief Compute control Jacobian matrix via numerical finite difference - * @param state Current ROE state vector - * @param control Current control vector - * @param time Current time - * @return Control Jacobian matrix (d f / d control) - */ - Eigen::MatrixXd getControlJacobian( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const override; - - /** - * @brief Compute state Hessian tensor - * @param state Current state vector - * @param control Current control vector - * @param time Current time - * @return Vector of state Hessian matrices, one per state dimension - */ - std::vector getStateHessian( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const override; - - /** - * @brief Compute control Hessian tensor - * @param state Current state vector - * @param control Current control vector - * @param time Current time - * @return Vector of control Hessian matrices, one per state dimension - */ - std::vector getControlHessian( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const override; - - /** - * @brief Transform the QNS-ROE state to the local Hill/Clohessy-Wiltshire frame. - * - * The returned vector is [x, y, z, xdot, ydot, zdot], representing the relative - * position and velocity in the Hill frame. - * - * @param roe A 6D QNS-ROE state vector - * @param t Time since epoch [s] - * @return A 6D vector [x, y, z, xdot, ydot, zdot] in the HCW frame - */ - Eigen::VectorXd transformROEToHCW(const Eigen::VectorXd &roe, double time) const; - - /** - * @brief Transform a HCW state to the QNS-ROE state. - * - * Expects a 6D input [x, y, z, xdot, ydot, zdot], representing the relative - * position and velocity in the Hill/Clohessy‐Wiltshire frame. - * - * @param hcw A 6D Hill/CWH state vector - * @param t Time since epoch [s] (used for the orbit phase) - * @return A 6D QNS-ROE vector [da, dlambda, dex, dey, dix, diy] - */ - Eigen::VectorXd transformHCWToROE(const Eigen::VectorXd &hcw, double t) const; - - double getSemiMajorAxis() const { return a_; } - double getMeanMotion() const { return n_ref_; } - double getInitialMeanArgumentOfLatitude() const { return u0_; } - double getGravitationalParameter() const { return mu_; } - void setGravitationalParameter(double mu) { mu_ = mu; } - void setMeanMotion(double n) { n_ref_ = n; } - void setSemiMajorAxis(double a) { a_ = a; } - void setInitialMeanArgumentOfLatitude(double u0) { u0_ = u0; } - - private: - double a_; ///< Semi-major axis of the reference orbit [m] - double n_ref_; ///< Mean motion of the reference orbit [rad/s] - double u0_; ///< Initial mean argu- ment of latitude [rad] - double mu_ = 3.9860044e14; ///< Gravitational parameter [m^3/s^2] - double mass_kg_; ///< Mass in kg - }; - -} // namespace cddp - -#endif // CDDP_SPACECRAFT_ROE_HPP diff --git a/pyproject.toml b/pyproject.toml index 967ea699..57e2b2eb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -12,7 +12,7 @@ requires-python = ">=3.10" dependencies = ["numpy>=1.22"] [project.optional-dependencies] -test = ["pytest>=7.0"] +test = ["pytest>=7.0", "matplotlib>=3.5"] viz = ["matplotlib>=3.5"] [dependency-groups] diff --git a/python/pycddp/__init__.py b/python/pycddp/__init__.py index 9ea7ddd4..afb4dd78 100644 --- a/python/pycddp/__init__.py +++ b/python/pycddp/__init__.py @@ -50,7 +50,6 @@ SpacecraftNonlinear, DreyfusRocket, SpacecraftLanding2D, - SpacecraftROE, SpacecraftTwobody, LTISystem, Usv3Dof, diff --git a/python/src/bind_dynamics.cpp b/python/src/bind_dynamics.cpp index 4efa7efd..fa1f277a 100644 --- a/python/src/bind_dynamics.cpp +++ b/python/src/bind_dynamics.cpp @@ -22,7 +22,6 @@ #include "dynamics_model/spacecraft_linear.hpp" #include "dynamics_model/spacecraft_linear_fuel.hpp" #include "dynamics_model/spacecraft_nonlinear.hpp" -#include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/spacecraft_twobody.hpp" #include "dynamics_model/unicycle.hpp" #include "dynamics_model/usv_3dof.hpp" @@ -226,11 +225,6 @@ void bind_dynamics(py::module_ &m) { py::arg("max_thrust") = 2210000.0, py::arg("max_gimble") = 0.349066); - py::class_(m, "SpacecraftROE") - .def(py::init(), - py::arg("timestep"), py::arg("integration_type"), py::arg("a"), - py::arg("u0") = 0.0, py::arg("mass_kg") = 1.0); - py::class_( m, "SpacecraftTwobody") .def(py::init(), py::arg("timestep"), diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 74b6b180..a653c4d4 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -47,7 +47,7 @@ bool nativeDynamicsCanSkipGil(const py::handle &object) { "QuadrotorRate", "Manipulator", "HCW", "SpacecraftLinearFuel", "SpacecraftNonlinear", "DreyfusRocket", "SpacecraftLanding2D", - "SpacecraftROE", "SpacecraftTwobody", + "SpacecraftTwobody", "LTISystem", "Usv3Dof", "EulerAttitude", "QuaternionAttitude", "MrpAttitude", }; diff --git a/python/tests/test_portfolio.py b/python/tests/test_portfolio.py new file mode 100644 index 00000000..3e0d3660 --- /dev/null +++ b/python/tests/test_portfolio.py @@ -0,0 +1,69 @@ +"""Regression tests for the Python portfolio demos.""" + +from __future__ import annotations + +import sys +from pathlib import Path + +import matplotlib +import numpy as np +import pytest + +matplotlib.use("Agg") + +sys.path.insert(0, str(Path(__file__).resolve().parents[2] / "examples")) + +import python_portfolio_lib as portfolio + + +def test_portfolio_demos_reach_targets(): + pendulum = portfolio.solve_pendulum_demo() + cartpole = portfolio.solve_cartpole_demo() + unicycle = portfolio.solve_unicycle_demo() + mpcc = portfolio.solve_mpcc_demo(simulation_steps=10, horizon=12) + + assert pendulum.solver_name == "CLDDP" + assert pendulum.final_error < 1e-3 + assert pendulum.states[:, 0].max() > 3.0 + assert abs(pendulum.controls[:, 0]).max() > 1.0 + + assert cartpole.solver_name == "CLDDP" + assert cartpole.final_error < 0.05 + + assert unicycle.solver_name == "IPDDP" + assert unicycle.final_error < 0.02 + assert unicycle.solution.final_primal_infeasibility < 1e-3 + + assert mpcc.solver_name.endswith("MPC") + assert mpcc.slug == "mpcc_racing_line" + assert mpcc.final_error < 0.5 + assert np.all(np.diff(mpcc.states[:, 3]) >= -1e-9) + assert np.max(np.abs(mpcc.metadata["contour_errors"])) < 0.85 + assert np.mean(mpcc.metadata["solve_times_ms"]) > 0.0 + + +def test_portfolio_animation_writes_gif(tmp_path): + result = portfolio.solve_pendulum_demo() + + output_path = tmp_path / "pendulum.gif" + written = portfolio.save_animation( + result, + output_path, + fps=10, + dpi=72, + frame_step=6, + ) + + assert written == output_path + assert output_path.exists() + assert output_path.stat().st_size > 0 + + +def test_portfolio_animation_rejects_invalid_settings(tmp_path): + result = portfolio.solve_pendulum_demo() + + with pytest.raises(ValueError, match="fps must be >= 1"): + portfolio.save_animation(result, tmp_path / "bad.gif", fps=0) + + with pytest.raises(ValueError, match="frame_step must be >= 1"): + portfolio.save_animation(result, tmp_path / "bad.gif", frame_step=0) diff --git a/results/acrobot/acrobot.gif b/results/acrobot/acrobot.gif deleted file mode 100644 index aea32c7f..00000000 Binary files a/results/acrobot/acrobot.gif and /dev/null differ diff --git a/results/animations/car_parking.gif b/results/animations/car_parking.gif deleted file mode 100644 index f4053de9..00000000 Binary files a/results/animations/car_parking.gif and /dev/null differ diff --git a/results/animations/car_parking_acado.gif b/results/animations/car_parking_acado.gif deleted file mode 100644 index 02a15755..00000000 Binary files a/results/animations/car_parking_acado.gif and /dev/null differ diff --git a/results/animations/car_parking_ipopt.gif b/results/animations/car_parking_ipopt.gif deleted file mode 100644 index 1cf9ff4e..00000000 Binary files a/results/animations/car_parking_ipopt.gif and /dev/null differ diff --git a/results/benchmark/quadrotor_computation_time_comparison.png b/results/benchmark/quadrotor_computation_time_comparison.png deleted file mode 100644 index 2105ac04..00000000 Binary files a/results/benchmark/quadrotor_computation_time_comparison.png and /dev/null differ diff --git a/results/benchmark/quadrotor_lemniscate_3d_comparison.png b/results/benchmark/quadrotor_lemniscate_3d_comparison.png deleted file mode 100644 index 7bd2b033..00000000 Binary files a/results/benchmark/quadrotor_lemniscate_3d_comparison.png and /dev/null differ diff --git a/results/benchmark/unicycle_baseline_trajectories_comparison.png b/results/benchmark/unicycle_baseline_trajectories_comparison.png deleted file mode 100644 index 48dcbce5..00000000 Binary files a/results/benchmark/unicycle_baseline_trajectories_comparison.png and /dev/null differ diff --git a/results/benchmark/unicycle_computation_time_comparison.png b/results/benchmark/unicycle_computation_time_comparison.png deleted file mode 100644 index c3520863..00000000 Binary files a/results/benchmark/unicycle_computation_time_comparison.png and /dev/null differ diff --git a/results/simulations/hcw_mpc_cddp_controls.png b/results/simulations/hcw_mpc_cddp_controls.png deleted file mode 100644 index e535cdcd..00000000 Binary files a/results/simulations/hcw_mpc_cddp_controls.png and /dev/null differ diff --git a/results/simulations/hcw_mpc_cddp_trajectories.png b/results/simulations/hcw_mpc_cddp_trajectories.png deleted file mode 100644 index af4ff99e..00000000 Binary files a/results/simulations/hcw_mpc_cddp_trajectories.png and /dev/null differ diff --git a/results/simulations/hcw_mpc_cddp_xaxis_trajectories.png b/results/simulations/hcw_mpc_cddp_xaxis_trajectories.png deleted file mode 100644 index cf42f52e..00000000 Binary files a/results/simulations/hcw_mpc_cddp_xaxis_trajectories.png and /dev/null differ diff --git a/results/simulations/hcw_mpc_cddp_yaxis_trajectories.png b/results/simulations/hcw_mpc_cddp_yaxis_trajectories.png deleted file mode 100644 index 886a85ce..00000000 Binary files a/results/simulations/hcw_mpc_cddp_yaxis_trajectories.png and /dev/null differ diff --git a/results/tests/bicycle.gif b/results/tests/bicycle.gif deleted file mode 100644 index b5e16d46..00000000 Binary files a/results/tests/bicycle.gif and /dev/null differ diff --git a/results/tests/bicycle_cddp_results.png b/results/tests/bicycle_cddp_results.png deleted file mode 100644 index e2705a9f..00000000 Binary files a/results/tests/bicycle_cddp_results.png and /dev/null differ diff --git a/results/tests/car_parking.gif b/results/tests/car_parking.gif deleted file mode 100644 index ef564d6c..00000000 Binary files a/results/tests/car_parking.gif and /dev/null differ diff --git a/results/tests/car_parking_ipddp.gif b/results/tests/car_parking_ipddp.gif deleted file mode 100644 index d6bc213c..00000000 Binary files a/results/tests/car_parking_ipddp.gif and /dev/null differ diff --git a/results/tests/car_parking_ipopt.gif b/results/tests/car_parking_ipopt.gif deleted file mode 100644 index f60abcc8..00000000 Binary files a/results/tests/car_parking_ipopt.gif and /dev/null differ diff --git a/results/tests/cartpole.gif b/results/tests/cartpole.gif deleted file mode 100644 index 979679d7..00000000 Binary files a/results/tests/cartpole.gif and /dev/null differ diff --git a/results/tests/cartpole_control_inputs.png b/results/tests/cartpole_control_inputs.png deleted file mode 100644 index b6f11784..00000000 Binary files a/results/tests/cartpole_control_inputs.png and /dev/null differ diff --git a/results/tests/cartpole_ipopt.gif b/results/tests/cartpole_ipopt.gif deleted file mode 100644 index 4d112dc9..00000000 Binary files a/results/tests/cartpole_ipopt.gif and /dev/null differ diff --git a/results/tests/cartpole_results.png b/results/tests/cartpole_results.png deleted file mode 100644 index 6727bab2..00000000 Binary files a/results/tests/cartpole_results.png and /dev/null differ diff --git a/results/tests/docking_trajectory.png b/results/tests/docking_trajectory.png deleted file mode 100644 index 12fd5a8d..00000000 Binary files a/results/tests/docking_trajectory.png and /dev/null differ diff --git a/results/tests/docking_trajectory_xy.png b/results/tests/docking_trajectory_xy.png deleted file mode 100644 index b129e2d0..00000000 Binary files a/results/tests/docking_trajectory_xy.png and /dev/null differ diff --git a/results/tests/dubincs_car_cddp_test.png b/results/tests/dubincs_car_cddp_test.png deleted file mode 100644 index 8ac9fd93..00000000 Binary files a/results/tests/dubincs_car_cddp_test.png and /dev/null differ diff --git a/results/tests/dubincs_car_clcddp_test.png b/results/tests/dubincs_car_clcddp_test.png deleted file mode 100644 index ef42581c..00000000 Binary files a/results/tests/dubincs_car_clcddp_test.png and /dev/null differ diff --git a/results/tests/dubincs_car_ipddp_test.png b/results/tests/dubincs_car_ipddp_test.png deleted file mode 100644 index 894b5d72..00000000 Binary files a/results/tests/dubincs_car_ipddp_test.png and /dev/null differ diff --git a/results/tests/dubincs_car_logcddp_test.png b/results/tests/dubincs_car_logcddp_test.png deleted file mode 100644 index 924ba738..00000000 Binary files a/results/tests/dubincs_car_logcddp_test.png and /dev/null differ diff --git a/results/tests/dubins_car.gif b/results/tests/dubins_car.gif deleted file mode 100644 index 7b837d3d..00000000 Binary files a/results/tests/dubins_car.gif and /dev/null differ diff --git a/results/tests/dubins_car_sqp_test.png b/results/tests/dubins_car_sqp_test.png deleted file mode 100644 index 7741d213..00000000 Binary files a/results/tests/dubins_car_sqp_test.png and /dev/null differ diff --git a/results/tests/forklift_parking_ipddp.gif b/results/tests/forklift_parking_ipddp.gif deleted file mode 100644 index a8041de4..00000000 Binary files a/results/tests/forklift_parking_ipddp.gif and /dev/null differ diff --git a/results/tests/hcw_control_history.png b/results/tests/hcw_control_history.png deleted file mode 100644 index 9ae040d9..00000000 Binary files a/results/tests/hcw_control_history.png and /dev/null differ diff --git a/results/tests/hcw_control_inputs.png b/results/tests/hcw_control_inputs.png deleted file mode 100644 index df121976..00000000 Binary files a/results/tests/hcw_control_inputs.png and /dev/null differ diff --git a/results/tests/hcw_results.png b/results/tests/hcw_results.png deleted file mode 100644 index c7573e82..00000000 Binary files a/results/tests/hcw_results.png and /dev/null differ diff --git a/results/tests/hcw_state_history.png b/results/tests/hcw_state_history.png deleted file mode 100644 index a0154156..00000000 Binary files a/results/tests/hcw_state_history.png and /dev/null differ diff --git a/results/tests/lti_states.png b/results/tests/lti_states.png deleted file mode 100644 index d6f53d29..00000000 Binary files a/results/tests/lti_states.png and /dev/null differ diff --git a/results/tests/manipulator.gif b/results/tests/manipulator.gif deleted file mode 100644 index ece42203..00000000 Binary files a/results/tests/manipulator.gif and /dev/null differ diff --git a/results/tests/manipulator_cddp_results.png b/results/tests/manipulator_cddp_results.png deleted file mode 100644 index 9160c01a..00000000 Binary files a/results/tests/manipulator_cddp_results.png and /dev/null differ diff --git a/results/tests/manipulator_pose.png b/results/tests/manipulator_pose.png deleted file mode 100644 index 0ff809bf..00000000 Binary files a/results/tests/manipulator_pose.png and /dev/null differ diff --git a/results/tests/pendulum.gif b/results/tests/pendulum.gif deleted file mode 100644 index ca97b6c9..00000000 Binary files a/results/tests/pendulum.gif and /dev/null differ diff --git a/results/tests/pendulum_cddp_test.png b/results/tests/pendulum_cddp_test.png deleted file mode 100644 index 3e9cf03e..00000000 Binary files a/results/tests/pendulum_cddp_test.png and /dev/null differ diff --git a/results/tests/quadrotor_circle.gif b/results/tests/quadrotor_circle.gif deleted file mode 100644 index 3910d44e..00000000 Binary files a/results/tests/quadrotor_circle.gif and /dev/null differ diff --git a/results/tests/quadrotor_circle_3d.png b/results/tests/quadrotor_circle_3d.png deleted file mode 100644 index bfc5d1d1..00000000 Binary files a/results/tests/quadrotor_circle_3d.png and /dev/null differ diff --git a/results/tests/quadrotor_circle_history.png b/results/tests/quadrotor_circle_history.png deleted file mode 100644 index e12ade82..00000000 Binary files a/results/tests/quadrotor_circle_history.png and /dev/null differ diff --git a/results/tests/quadrotor_circle_states.png b/results/tests/quadrotor_circle_states.png deleted file mode 100644 index 3082b02b..00000000 Binary files a/results/tests/quadrotor_circle_states.png and /dev/null differ diff --git a/results/tests/quadrotor_discrete_dynamics.png b/results/tests/quadrotor_discrete_dynamics.png deleted file mode 100644 index 942e66c4..00000000 Binary files a/results/tests/quadrotor_discrete_dynamics.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_horizontal.gif b/results/tests/quadrotor_figure_eight_horizontal.gif deleted file mode 100644 index e9908d21..00000000 Binary files a/results/tests/quadrotor_figure_eight_horizontal.gif and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_horizontal_3d.png b/results/tests/quadrotor_figure_eight_horizontal_3d.png deleted file mode 100644 index caf02a09..00000000 Binary files a/results/tests/quadrotor_figure_eight_horizontal_3d.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_horizontal_safe_3d.png b/results/tests/quadrotor_figure_eight_horizontal_safe_3d.png deleted file mode 100644 index 62e79732..00000000 Binary files a/results/tests/quadrotor_figure_eight_horizontal_safe_3d.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_horizontal_safe_states.png b/results/tests/quadrotor_figure_eight_horizontal_safe_states.png deleted file mode 100644 index f5d679e8..00000000 Binary files a/results/tests/quadrotor_figure_eight_horizontal_safe_states.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_horizontal_states.png b/results/tests/quadrotor_figure_eight_horizontal_states.png deleted file mode 100644 index 04a3c011..00000000 Binary files a/results/tests/quadrotor_figure_eight_horizontal_states.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_vertical.gif b/results/tests/quadrotor_figure_eight_vertical.gif deleted file mode 100644 index c3eec65b..00000000 Binary files a/results/tests/quadrotor_figure_eight_vertical.gif and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_vertical_3d.png b/results/tests/quadrotor_figure_eight_vertical_3d.png deleted file mode 100644 index dab010a8..00000000 Binary files a/results/tests/quadrotor_figure_eight_vertical_3d.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_vertical_states.png b/results/tests/quadrotor_figure_eight_vertical_states.png deleted file mode 100644 index 2f4b643c..00000000 Binary files a/results/tests/quadrotor_figure_eight_vertical_states.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_xy.png b/results/tests/quadrotor_figure_eight_xy.png deleted file mode 100644 index a7bbf38d..00000000 Binary files a/results/tests/quadrotor_figure_eight_xy.png and /dev/null differ diff --git a/results/tests/quadrotor_figure_eight_xz.png b/results/tests/quadrotor_figure_eight_xz.png deleted file mode 100644 index e982d197..00000000 Binary files a/results/tests/quadrotor_figure_eight_xz.png and /dev/null differ diff --git a/results/tests/quadrotor_point.gif b/results/tests/quadrotor_point.gif deleted file mode 100644 index 42b3693a..00000000 Binary files a/results/tests/quadrotor_point.gif and /dev/null differ diff --git a/results/tests/quadrotor_point_3d.png b/results/tests/quadrotor_point_3d.png deleted file mode 100644 index 9d540eeb..00000000 Binary files a/results/tests/quadrotor_point_3d.png and /dev/null differ diff --git a/results/tests/quadrotor_point_history.png b/results/tests/quadrotor_point_history.png deleted file mode 100644 index 817d332a..00000000 Binary files a/results/tests/quadrotor_point_history.png and /dev/null differ diff --git a/results/tests/quadrotor_point_states.png b/results/tests/quadrotor_point_states.png deleted file mode 100644 index 616419fc..00000000 Binary files a/results/tests/quadrotor_point_states.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_ipddp.png b/results/tests/trajectory_comparison_ipddp.png deleted file mode 100644 index 07ade010..00000000 Binary files a/results/tests/trajectory_comparison_ipddp.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_ipddp_matplot.png b/results/tests/trajectory_comparison_ipddp_matplot.png deleted file mode 100644 index f8995389..00000000 Binary files a/results/tests/trajectory_comparison_ipddp_matplot.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_ipddp_v2.png b/results/tests/trajectory_comparison_ipddp_v2.png deleted file mode 100644 index 161bbb55..00000000 Binary files a/results/tests/trajectory_comparison_ipddp_v2.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_ipddp_v2_matplot.png b/results/tests/trajectory_comparison_ipddp_v2_matplot.png deleted file mode 100644 index 7fb03743..00000000 Binary files a/results/tests/trajectory_comparison_ipddp_v2_matplot.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_ipddp_v3_linear_matplot.png b/results/tests/trajectory_comparison_ipddp_v3_linear_matplot.png deleted file mode 100644 index 621ad1d2..00000000 Binary files a/results/tests/trajectory_comparison_ipddp_v3_linear_matplot.png and /dev/null differ diff --git a/results/tests/trajectory_comparison_matplot.png b/results/tests/trajectory_comparison_matplot.png deleted file mode 100644 index 4283b42a..00000000 Binary files a/results/tests/trajectory_comparison_matplot.png and /dev/null differ diff --git a/results/tests/unicycle.gif b/results/tests/unicycle.gif deleted file mode 100644 index 2cc86eba..00000000 Binary files a/results/tests/unicycle.gif and /dev/null differ diff --git a/results/tests/unicycle_ascddp_test.png b/results/tests/unicycle_ascddp_test.png deleted file mode 100644 index a8a155b7..00000000 Binary files a/results/tests/unicycle_ascddp_test.png and /dev/null differ diff --git a/results/tests/unicycle_cddp_results.png b/results/tests/unicycle_cddp_results.png deleted file mode 100644 index 02d3d790..00000000 Binary files a/results/tests/unicycle_cddp_results.png and /dev/null differ diff --git a/results/tests/unicycle_ipopt.gif b/results/tests/unicycle_ipopt.gif deleted file mode 100644 index 1857c335..00000000 Binary files a/results/tests/unicycle_ipopt.gif and /dev/null differ diff --git a/results/tests/unicycle_ipopt_results.png b/results/tests/unicycle_ipopt_results.png deleted file mode 100644 index 2f46732d..00000000 Binary files a/results/tests/unicycle_ipopt_results.png and /dev/null differ diff --git a/results/tests/unicycle_safe.gif b/results/tests/unicycle_safe.gif deleted file mode 100644 index 9b71c74d..00000000 Binary files a/results/tests/unicycle_safe.gif and /dev/null differ diff --git a/results/tests/unicycle_six_trajectories_comparison.png b/results/tests/unicycle_six_trajectories_comparison.png deleted file mode 100644 index 8b619831..00000000 Binary files a/results/tests/unicycle_six_trajectories_comparison.png and /dev/null differ diff --git a/results/tests/unicycle_trajectory_comparison.png b/results/tests/unicycle_trajectory_comparison.png deleted file mode 100644 index cce977b9..00000000 Binary files a/results/tests/unicycle_trajectory_comparison.png and /dev/null differ diff --git a/src/dynamics_model/spacecraft_roe.cpp b/src/dynamics_model/spacecraft_roe.cpp deleted file mode 100644 index 71fe4704..00000000 --- a/src/dynamics_model/spacecraft_roe.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/** - * @file cddp_spacecraft_roe.cpp - * @author ... - * @date 2024 - * @brief Implementation of SpacecraftROE for QNS-ROE dynamics. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - */ - -#include "dynamics_model/spacecraft_roe.hpp" - -#include -#include -#include -#include - -namespace cddp -{ - - SpacecraftROE::SpacecraftROE( - double timestep, - const std::string &integration_type, - double a, - double u0, - double mass_kg) - : DynamicalSystem(STATE_DIM, CONTROL_DIM, timestep, integration_type), - a_(a), - u0_(u0), - mass_kg_(mass_kg) - { - - n_ref_ = std::sqrt(mu_ / (a_ * a_ * a_)); // Mean motion of the reference orbit - } - - //----------------------------------------------------------------------------- - Eigen::VectorXd SpacecraftROE::getContinuousDynamics( - const Eigen::VectorXd &state, - const Eigen::VectorXd &control, double time) const - { - /** - * Based on the linear QNSROE model: - * ẋ = A x + B u - * - * x = [ da, dlambda, dex, dey, dix, diy] - * u = [ ur, ut, un ] - * - * A = [ 0 0 0 0 0 0 0 - * -3/2 * n_ref_ 0 0 0 0 0 - * 0 0 0 0 0 0 - * 0 0 0 0 0 0 - * 0 0 0 0 0 0 - * 0 0 0 0 0 0 ] - * - * B(t) = 1/(n_ref_*a_) * - * [ 0 2 0 - * -2 0 0 - * su 2cu 0 - * -cu 2su 0 - * 0 0 cu - * 0 0 su ] - */ - - // Create a zero derivative vector - Eigen::VectorXd xdot = Eigen::VectorXd::Zero(STATE_DIM); - - // Current argument of latitude - double nu = n_ref_ * time + u0_; - - // A*x portion: - double da = state(STATE_DA); - xdot(STATE_DLAMBDA) = -1.5 * n_ref_ * da; - - // B*u portion: - double ur = control(CONTROL_UR); - double ut = control(CONTROL_UT); - double un = control(CONTROL_UN); - - // Factor for scaling control - const double factor = 1.0 / (n_ref_ * a_ * mass_kg_); - - // Calculate su and cu based on initial argument of latitude: - double su = std::sin(nu); - double cu = std::cos(nu); - - // Return the control Jacobian matrix B - Eigen::MatrixXd B(STATE_DIM, CONTROL_DIM); - B.setZero(); - - B(STATE_DA, CONTROL_UT) = 2.0; - B(STATE_DLAMBDA, CONTROL_UR) = -2.0; - B(STATE_DEX, CONTROL_UR) = su; - B(STATE_DEX, CONTROL_UT) = 2.0 * cu; - B(STATE_DEY, CONTROL_UR) = -cu; - B(STATE_DEY, CONTROL_UT) = 2.0 * su; - B(STATE_DIX, CONTROL_UN) = cu; - B(STATE_DIY, CONTROL_UN) = su; - B *= factor; - - return xdot + B * control; - } - - VectorXdual2nd SpacecraftROE::getContinuousDynamicsAutodiff( - const VectorXdual2nd &state, const VectorXdual2nd &control, double time) const - { - VectorXdual2nd xdot = VectorXdual2nd::Zero(STATE_DIM); - - // Current argument of latitude - double nu = n_ref_ * time + u0_; - - // A*x portion: - autodiff::dual2nd da = state(STATE_DA); - xdot(STATE_DLAMBDA) = -1.5 * n_ref_ * da; - - // B*u portion: - autodiff::dual2nd ur = control(CONTROL_UR); - autodiff::dual2nd ut = control(CONTROL_UT); - autodiff::dual2nd un = control(CONTROL_UN); - - // Factor for scaling control - const double factor = 1.0 / (n_ref_ * a_ * mass_kg_); - - // Calculate su and cu based on initial argument of latitude: - double su = std::sin(nu); - double cu = std::cos(nu); - - // Return the control Jacobian matrix B - Eigen::MatrixXd B(STATE_DIM, CONTROL_DIM); - B.setZero(); - - B(STATE_DA, CONTROL_UT) = 2.0; - B(STATE_DLAMBDA, CONTROL_UR) = -2.0; - B(STATE_DEX, CONTROL_UR) = su; - B(STATE_DEX, CONTROL_UT) = 2.0 * cu; - B(STATE_DEY, CONTROL_UR) = -cu; - B(STATE_DEY, CONTROL_UT) = 2.0 * su; - B(STATE_DIX, CONTROL_UN) = cu; - B(STATE_DIY, CONTROL_UN) = su; - B *= factor; - - return xdot + B * control; - } - - //----------------------------------------------------------------------------- - Eigen::MatrixXd SpacecraftROE::getStateJacobian( - const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const - { - - // Use autodiff to compute state Jacobian - VectorXdual2nd x = state; - VectorXdual2nd u = control; - - auto dynamics_wrt_x = [&](const VectorXdual2nd &x_ad) -> VectorXdual2nd - { - return this->getContinuousDynamicsAutodiff(x_ad, u, time); - }; - - return autodiff::jacobian(dynamics_wrt_x, wrt(x), at(x)); - } - - Eigen::MatrixXd SpacecraftROE::getControlJacobian( - const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const - { - - // Use autodiff to compute control Jacobian - VectorXdual2nd x = state; - VectorXdual2nd u = control; - - auto dynamics_wrt_u = [&](const VectorXdual2nd &u_ad) -> VectorXdual2nd - { - return this->getContinuousDynamicsAutodiff(x, u_ad, time); - }; - - return autodiff::jacobian(dynamics_wrt_u, wrt(u), at(u)); - } - - //----------------------------------------------------------------------------- - std::vector SpacecraftROE::getStateHessian( - const Eigen::VectorXd & /*state*/, - const Eigen::VectorXd & /*control*/, double time) const - { - // For this linear(ish) model, second derivatives wrt state are zero. - auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - return hessians; - } - - //----------------------------------------------------------------------------- - std::vector SpacecraftROE::getControlHessian( - const Eigen::VectorXd & /*state*/, - const Eigen::VectorXd & /*control*/, double time) const - { - // Similarly, second derivatives wrt control are zero for a linear system. - auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - return hessians; - } - - //----------------------------------------------------------------------------- - Eigen::VectorXd SpacecraftROE::transformROEToHCW(const Eigen::VectorXd &roe, double t) const - { - // 1) Compute the angle for reference orbit at time t - double phi = n_ref_ * t + u0_; - double cn = std::cos(phi); - double sn = std::sin(phi); - - Eigen::Matrix T; - T.setZero(); - - // Fill row by row (0-based indexing). - // Position components - // Row 0: [1, 0, -cn, -sn, 0, 0] * a_ - T(0, 0) = 1.0; - T(0, 1) = 0.0; - T(0, 2) = -cn; - T(0, 3) = -sn; - T(0, 4) = 0.0; - T(0, 5) = 0.0; - - // Row 1: [0, 1, 2sn, -2cn, 0, 0] * a_ - T(1, 0) = 0.0; - T(1, 1) = 1.0; - T(1, 2) = 2.0 * sn; - T(1, 3) = -2.0 * cn; - T(1, 4) = 0.0; - T(1, 5) = 0.0; - - // Row 2: [0, 0, 0, 0, sn, -cn] * a_ - T(2, 0) = 0.0; - T(2, 1) = 0.0; - T(2, 2) = 0.0; - T(2, 3) = 0.0; - T(2, 4) = sn; - T(2, 5) = -cn; - - // Velocity components - // Row 3: [0, 0, n*sn, -n*cn, 0, 0] - T(3, 0) = 0.0; - T(3, 1) = 0.0; - T(3, 2) = n_ref_ * sn; - T(3, 3) = -n_ref_ * cn; - T(3, 4) = 0.0; - T(3, 5) = 0.0; - - // Row 4: [-3n/2, 0, 2n*cn, 2n*sn, 0, 0] - T(4, 0) = -1.5 * n_ref_; - T(4, 1) = 0.0; - T(4, 2) = 2.0 * n_ref_ * cn; - T(4, 3) = 2.0 * n_ref_ * sn; - T(4, 4) = 0.0; - T(4, 5) = 0.0; - - // Row 5: [0, 0, 0, 0, n*cn, n*sn] - T(5, 0) = 0.0; - T(5, 1) = 0.0; - T(5, 2) = 0.0; - T(5, 3) = 0.0; - T(5, 4) = n_ref_ * cn; - T(5, 5) = n_ref_ * sn; - - // Multiply entire matrix by 'a_' - T *= a_; - - // 3) Multiply T by the input ROE vector - Eigen::VectorXd hcw = T * roe; // hcw is [x, y, z, xdot, ydot, zdot] - - return hcw; - } - - Eigen::VectorXd SpacecraftROE::transformHCWToROE(const Eigen::VectorXd &hcw, double t) const - { - // 1) Compute the orbital phase for the reference orbit at time t - double phi = n_ref_ * t + u0_; - double cn = std::cos(phi); - double sn = std::sin(phi); - - // 2) Build the inverse transformation matrix Tinv (6x6) - // - // Tinv = [4 0 0 0 2/n 0 - // 0 1 0 -2/n 0 0 - // 3cn 0 0 sn/n 2cn/n 0 - // 3sn 0 0 -cn/n 2sn/n 0 - // 0 0 sn 0 0 cn/n - // 0 0 -cn 0 0 sn/n ] / a - // - - Eigen::Matrix Tinv; - Tinv.setZero(); - - // row 0 - Tinv(0, 0) = 4.0; - Tinv(0, 1) = 0.0; - Tinv(0, 2) = 0.0; - Tinv(0, 3) = 0.0; - Tinv(0, 4) = 2.0 / n_ref_; - Tinv(0, 5) = 0.0; - - // row 1 - Tinv(1, 0) = 0.0; - Tinv(1, 1) = 1.0; - Tinv(1, 2) = 0.0; - Tinv(1, 3) = -2.0 / n_ref_; - Tinv(1, 4) = 0.0; - Tinv(1, 5) = 0.0; - - // row 2 - Tinv(2, 0) = 3.0 * cn; - Tinv(2, 1) = 0.0; - Tinv(2, 2) = 0.0; - Tinv(2, 3) = sn / n_ref_; - Tinv(2, 4) = 2.0 * cn / n_ref_; - Tinv(2, 5) = 0.0; - - // row 3 - Tinv(3, 0) = 3.0 * sn; - Tinv(3, 1) = 0.0; - Tinv(3, 2) = 0.0; - Tinv(3, 3) = -cn / n_ref_; - Tinv(3, 4) = 2.0 * sn / n_ref_; - Tinv(3, 5) = 0.0; - - // row 4 - Tinv(4, 0) = 0.0; - Tinv(4, 1) = 0.0; - Tinv(4, 2) = sn; - Tinv(4, 3) = 0.0; - Tinv(4, 4) = 0.0; - Tinv(4, 5) = cn / n_ref_; - - // row 5 - Tinv(5, 0) = 0.0; - Tinv(5, 1) = 0.0; - Tinv(5, 2) = -cn; - Tinv(5, 3) = 0.0; - Tinv(5, 4) = 0.0; - Tinv(5, 5) = sn / n_ref_; - - // Divide entire matrix by a_ - Tinv /= a_; - - // 3) Multiply Tinv by the input HCW vector => QNS-ROE - Eigen::VectorXd roe_6dim = Tinv * hcw; - - return roe_6dim; - } - -} // namespace cddp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 68fb1003..265781ba 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -171,10 +171,6 @@ gtest_discover_tests(test_msipddp_solver) # target_link_libraries(test_ipddp_quadrotor gtest gmock gtest_main cddp) # gtest_discover_tests(test_ipddp_quadrotor) -# add_executable(test_spacecraft_roe dynamics_model/test_spacecraft_roe.cpp) -# target_link_libraries(test_spacecraft_roe gtest gmock gtest_main cddp) -# gtest_discover_tests(test_spacecraft_roe) - if (CDDP_CPP_CASADI) add_executable(test_casadi test_casadi_solver.cpp) target_link_libraries(test_casadi gtest gmock gtest_main cddp) diff --git a/tests/dynamics_model/test_spacecraft_roe.cpp b/tests/dynamics_model/test_spacecraft_roe.cpp deleted file mode 100644 index e724250d..00000000 --- a/tests/dynamics_model/test_spacecraft_roe.cpp +++ /dev/null @@ -1,248 +0,0 @@ -/** - * @file test_spacecraft_roe.cpp - * @brief GTest unit tests for the SpacecraftROE class (QNS-ROE dynamics). - * - * Copyright 2024 - * Licensed under the Apache License, Version 2.0 (the "License"); - * See the LICENSE file in the project root for more details. - */ - -#include -#include -#include -#include -#include - -#include "dynamics_model/spacecraft_roe.hpp" -#include "dynamics_model/spacecraft_linear.hpp" -using namespace cddp; - -TEST(TestSpacecraftROE, BasicQNSROEPropagation) -{ - // 1) Define problem parameters - double a = (6378.0 + 400.0) * 1e3; // semi-major axis [m] - double period = 2 * M_PI * sqrt(pow(a, 3) / 3.9860044e14); // orbital period [s] - - // 2) Create SpacecraftROE instance - double dt = 10.0; // time step - std::string integration_type = "euler"; - double u0 = 0.0; // initial argument of latitude - SpacecraftROE qnsRoeModel(dt, integration_type, a, u0); - - // 3) Create an initial relative state in the local RTN or Hill frame (6D) - Eigen::VectorXd x0_RV(6); - x0_RV << 495.08966689311296, -200.0, - 6.123233995736766e-15, -0.0, - -1.1230676194377847, -0.11314009527711179; - - // 4) Convert from R/V to QNSROE - Eigen::VectorXd x0_roe = qnsRoeModel.transformHCWToROE(x0_RV, 0.0); - - // 5) Integrate from t=0 to t=2*period - std::vector times; - std::vector states; - int num_steps = 2*period / dt; // 3 hours of simulation - - Eigen::VectorXd x_roe = x0_roe; - - for (int i = 0; i < num_steps; ++i) - { - double t = i * dt; - x_roe = qnsRoeModel.getDiscreteDynamics(x_roe, Eigen::VectorXd::Zero(SpacecraftROE::CONTROL_DIM), 0.0); - times.push_back(t); - states.push_back(x_roe); - } - - // 6) Print - std::cout << "[ INFO ] Initial QNS-ROE state:\n" - << x0_roe.transpose() * a << std::endl; - std::cout << "[ INFO ] Initial time: " << times.front() << " s" << std::endl; - std::cout << "[ INFO ] Final time: " << times.back() << " s" << std::endl; - std::cout << "[ INFO ] Final QNS-ROE state:\n" - << states.back().transpose() * a << std::endl; - - // 7) Basic sanity checks - - Eigen::VectorXd expected_x0_roe(6), expected_xf_roe(6); - expected_x0_roe << -4.910333106887314, -200.0, -500.0000000000003, 0.0, -100.0, -6.123233995736765e-15; - expected_xf_roe << -4.910333106887314, -107.44240150834467, -500.0000000000003, 0.0, -100.0, -6.123233995736765e-15; - Eigen::VectorXd obtained_x0_roe = states.front().head(6) * a; - Eigen::VectorXd obtained_xf_roe = states.back().head(6) * a; - ASSERT_EQ(states.front().size(), SpacecraftROE::STATE_DIM); - ASSERT_EQ(states.back().size(), SpacecraftROE::STATE_DIM); - ASSERT_NEAR(obtained_x0_roe.norm(), expected_x0_roe.norm(), 1e-1); - ASSERT_NEAR(obtained_xf_roe.norm(), expected_xf_roe.norm(), 1e-1); -} - -TEST(TestSpacecraftROE, RelativeTrajectory) -{ - // 1) Define problem parameters - double a = (6371.0 + 500.0) * 1e3; // semi-major axis [m] - double mean_motion = std::sqrt(3.986004418e14 / std::pow(a, 3)); // For 500km orbit - double period = 2 * M_PI / mean_motion; // orbital period [s] - - // 2) Create SpacecraftROE instance - double dt = 10.0; // time step - std::string integration_type = "euler"; - double u0 = 0.0; // initial argument of latitude - SpacecraftROE qnsRoeModel(dt, integration_type, a, u0); - - // 3) Initial HCW state - Eigen::VectorXd x0_hcw(6); - x0_hcw << -37.59664132226163, - 27.312455860666148, - 13.656227930333074, - 0.015161970413423813, - 0.08348413138390476, - 0.04174206569195238; - - // 4) Convert initial HCW to ROE - Eigen::VectorXd x0_roe = qnsRoeModel.transformHCWToROE(x0_hcw, 0.0); - - std::cout << "[ INFO ] Initial HCW state:\n" - << x0_hcw.transpose() << std::endl; - std::cout << "[ INFO ] Initial ROE state:\n" - << x0_roe.transpose() << std::endl; - - // 5) Simulate for a portion of an orbit - int num_steps = static_cast(3.0 * period / dt); // Simulate for 3 orbits - Eigen::VectorXd current_roe_state = x0_roe; - Eigen::VectorXd control = Eigen::VectorXd::Zero(SpacecraftROE::CONTROL_DIM); // No control - std::cout << "mean_motion: " << mean_motion << std::endl; - std::cout << "timestep: " << dt << std::endl; - std::cout << "period: " << period << std::endl; - std::cout << "num_steps: " << num_steps << std::endl; - std::vector roe_trajectory; - std::vector hcw_trajectory; - roe_trajectory.push_back(current_roe_state); - hcw_trajectory.push_back(qnsRoeModel.transformROEToHCW(current_roe_state.head(6), 0.0)); - - for (int i = 0; i < num_steps; ++i) { - double t = i * dt; - current_roe_state = qnsRoeModel.getDiscreteDynamics(current_roe_state, control, t); - roe_trajectory.push_back(current_roe_state); - hcw_trajectory.push_back(qnsRoeModel.transformROEToHCW(current_roe_state.head(6), t)); - } - - // Print the final HCW state - std::cout << "[ INFO ] Final HCW state:\n" - << hcw_trajectory.back().transpose() << std::endl; - std::cout << "[ INFO ] Final ROE state:\n" - << roe_trajectory.back().transpose() * a<< std::endl; - - // 6) Basic Assertions - ASSERT_EQ(roe_trajectory.size(), num_steps + 1); - ASSERT_EQ(hcw_trajectory.size(), num_steps + 1); - - // Check that the initial HCW state, when converted to ROE and back to HCW, is consistent. - Eigen::VectorXd x0_hcw_reconstructed = qnsRoeModel.transformROEToHCW(x0_roe.head(6), 0.0); - for (int i = 0; i < 6; ++i) { - ASSERT_NEAR(x0_hcw(i), x0_hcw_reconstructed(i), 1e-9); - } - - ASSERT_GT(hcw_trajectory.back().norm(), 1e-3); - -} - -TEST(TestSpacecraftROE, JacobianBasedPropagation) -{ - // 1) Define problem parameters - double a_param = (6378.0 + 400.0) * 1e3; // semi-major axis [m] - double dt = 1.0; // time step [s] - std::string integration_type = "euler"; - double u0_model_phase = 0.1; // initial argument of latitude for the model [rad] - double mass_kg = 1.0; // mass of spacecraft [kg] - - // 2) Create SpacecraftROE instance - cddp::SpacecraftROE qnsRoeModel(dt, integration_type, a_param, u0_model_phase, mass_kg); - ASSERT_EQ(qnsRoeModel.getStateDim(), cddp::SpacecraftROE::STATE_DIM); - ASSERT_EQ(qnsRoeModel.getControlDim(), cddp::SpacecraftROE::CONTROL_DIM); - - // 3) Define initial state (7D ROE state) - Eigen::VectorXd x0(cddp::SpacecraftROE::STATE_DIM); - x0.setZero(); - // Dimensionless ROE elements + initial true anomaly - x0(cddp::SpacecraftROE::STATE_DA) = 100.0 / a_param; - x0(cddp::SpacecraftROE::STATE_DLAMBDA) = 0.0001; // rad - x0(cddp::SpacecraftROE::STATE_DEX) = 50.0 / a_param; - x0(cddp::SpacecraftROE::STATE_DEY) = -50.0 / a_param; - x0(cddp::SpacecraftROE::STATE_DIX) = 20.0 / a_param; - x0(cddp::SpacecraftROE::STATE_DIY) = -20.0 / a_param; - - // --- Test with Zero Control --- - Eigen::VectorXd u_zero_control(cddp::SpacecraftROE::CONTROL_DIM); - u_zero_control.setZero(); - - - // 6) Simulate trajectories - int num_steps = 20; - std::vector x_trajectory_dyn_zero_u; - std::vector x_trajectory_lin_zero_u; - - Eigen::VectorXd x_curr_dyn_zero_u = x0; - Eigen::VectorXd x_curr_lin_zero_u = x0; - - x_trajectory_dyn_zero_u.push_back(x_curr_dyn_zero_u); - x_trajectory_lin_zero_u.push_back(x_curr_lin_zero_u); - - for (int k = 0; k < num_steps; ++k) - { - x_curr_dyn_zero_u = qnsRoeModel.getDiscreteDynamics(x_curr_dyn_zero_u, u_zero_control, k * dt); - x_trajectory_dyn_zero_u.push_back(x_curr_dyn_zero_u); - - Eigen::MatrixXd A = qnsRoeModel.getStateJacobian(x_curr_lin_zero_u, u_zero_control, k * dt); - Eigen::MatrixXd B = qnsRoeModel.getControlJacobian(x_curr_lin_zero_u, u_zero_control, k * dt); - x_curr_lin_zero_u = x_curr_lin_zero_u + dt * (A * x_curr_lin_zero_u + B * u_zero_control); - x_trajectory_lin_zero_u.push_back(x_curr_lin_zero_u); - } - - ASSERT_EQ(x_trajectory_dyn_zero_u.size(), num_steps + 1); - ASSERT_EQ(x_trajectory_lin_zero_u.size(), num_steps + 1); - - for (int j = 0; j < cddp::SpacecraftROE::STATE_DIM; ++j) { - ASSERT_NEAR(x_trajectory_dyn_zero_u[1](j), x_trajectory_lin_zero_u[1](j), 1e-2); - } - Eigen::VectorXd diff_final_zero_u = x_trajectory_dyn_zero_u.back() - x_trajectory_lin_zero_u.back(); - EXPECT_LT(diff_final_zero_u.norm(), 1e-2); - - - // --- Test with Non-Zero Control --- - Eigen::VectorXd u_nonzero_control(cddp::SpacecraftROE::CONTROL_DIM); - u_nonzero_control << 1e-5, -2e-5, 1.5e-5; // Small accelerations in m/s^2 - - - std::vector x_trajectory_dyn_nonzero_u; - std::vector x_trajectory_lin_nonzero_u; - - Eigen::VectorXd x_curr_dyn_nonzero_u = x0; - Eigen::VectorXd x_curr_lin_nonzero_u = x0; - - x_trajectory_dyn_nonzero_u.push_back(x_curr_dyn_nonzero_u); - x_trajectory_lin_nonzero_u.push_back(x_curr_lin_nonzero_u); - - for (int k = 0; k < num_steps; ++k) - { - x_curr_dyn_nonzero_u = qnsRoeModel.getDiscreteDynamics(x_curr_dyn_nonzero_u, u_nonzero_control, k * dt); - x_trajectory_dyn_nonzero_u.push_back(x_curr_dyn_nonzero_u); - - Eigen::MatrixXd A = qnsRoeModel.getStateJacobian(x_curr_lin_nonzero_u, u_nonzero_control, k * dt); - Eigen::MatrixXd B = qnsRoeModel.getControlJacobian(x_curr_lin_nonzero_u, u_nonzero_control, k * dt); - x_curr_lin_nonzero_u = x_curr_lin_nonzero_u + dt * (A * x_curr_lin_nonzero_u + B * u_nonzero_control); - x_trajectory_lin_nonzero_u.push_back(x_curr_lin_nonzero_u); - } - - ASSERT_EQ(x_trajectory_dyn_nonzero_u.size(), num_steps + 1); - ASSERT_EQ(x_trajectory_lin_nonzero_u.size(), num_steps + 1); - - for (int j = 0; j < cddp::SpacecraftROE::STATE_DIM; ++j) { - ASSERT_NEAR(x_trajectory_dyn_nonzero_u[1](j), x_trajectory_lin_nonzero_u[1](j), 1e-12); - } - Eigen::VectorXd diff_final_nonzero_u = x_trajectory_dyn_nonzero_u.back() - x_trajectory_lin_nonzero_u.back(); - EXPECT_LT(diff_final_nonzero_u.norm(), 1e-4 * num_steps); -} - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}