diff --git a/404.html b/404.html index fc31de341bde57e273852bd8b75fcc89cad3dff4..0e05b24553b5802bcd6cb73994870e83cda0c099 100644 --- a/404.html +++ b/404.html @@ -51,6 +51,11 @@ <a class="" href="/build_doc/">Build documentation</a> </li> + <li class="toctree-l1"> + + <a class="" href="/example/">Example</a> + </li> + </ul> </div> @@ -118,6 +123,8 @@ </div> <script>var base_url = '/';</script> <script src="/js/theme.js" defer></script> + <script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML" defer></script> + <script src="/docs/mathjaxhelper.js" defer></script> <script src="/search/main.js" defer></script> </body> diff --git a/build_doc/index.html b/build_doc/index.html index 1fafcb7719336c0066c4cfb7df3169a2fc910512..e53581f6c7fff1a2fc835eb04ce82183ac0b4a9a 100644 --- a/build_doc/index.html +++ b/build_doc/index.html @@ -74,6 +74,11 @@ </ul> </li> + <li class="toctree-l1"> + + <a class="" href="../example/">Example</a> + </li> + </ul> </div> @@ -109,7 +114,7 @@ <h1 id="build-documentation-using-mkdocs">Build documentation using MkDocs</h1> <p>This project uses <a href="https://www.mkdocs.org">MkDocs</a> for building the documentation site.</p> <h2 id="installation">Installation</h2> -<pre><code class="bash">pip install mkdocs +<pre><code class="bash">pip install mkdocs python-markdown-math </code></pre> <h2 id="build-and-serve-locally">Build and serve locally</h2> @@ -133,6 +138,8 @@ mkdocs gh-deploy <div class="rst-footer-buttons" role="navigation" aria-label="footer navigation"> + <a href="../example/" class="btn btn-neutral float-right" title="Example">Next <span class="icon icon-circle-arrow-right"></span></a> + <a href=".." class="btn btn-neutral" title="Home"><span class="icon icon-circle-arrow-left"></span> Previous</a> @@ -163,10 +170,14 @@ mkdocs gh-deploy <span><a href=".." style="color: #fcfcfc;">« Previous</a></span> + <span style="margin-left: 15px"><a href="../example/" style="color: #fcfcfc">Next »</a></span> + </span> </div> <script>var base_url = '..';</script> <script src="../js/theme.js" defer></script> + <script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML" defer></script> + <script src="../docs/mathjaxhelper.js" defer></script> <script src="../search/main.js" defer></script> </body> diff --git a/example/index.html b/example/index.html new file mode 100644 index 0000000000000000000000000000000000000000..193a25f099060b953a3d713f8323cf629bdd7762 --- /dev/null +++ b/example/index.html @@ -0,0 +1,337 @@ +<!DOCTYPE html> +<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]--> +<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]--> +<head> + <meta charset="utf-8"> + <meta http-equiv="X-UA-Compatible" content="IE=edge"> + <meta name="viewport" content="width=device-width, initial-scale=1.0"> + + + <link rel="shortcut icon" href="../img/favicon.ico"> + <title>Example - PolyMPC</title> + <link href='https://fonts.googleapis.com/css?family=Lato:400,700|Roboto+Slab:400,700|Inconsolata:400,700' rel='stylesheet' type='text/css'> + + <link rel="stylesheet" href="../css/theme.css" type="text/css" /> + <link rel="stylesheet" href="../css/theme_extra.css" type="text/css" /> + <link rel="stylesheet" href="//cdnjs.cloudflare.com/ajax/libs/highlight.js/9.12.0/styles/github.min.css"> + + <script> + // Current page data + var mkdocs_page_name = "Example"; + var mkdocs_page_input_path = "example.md"; + var mkdocs_page_url = null; + </script> + + <script src="../js/jquery-2.1.1.min.js" defer></script> + <script src="../js/modernizr-2.8.3.min.js" defer></script> + <script src="//cdnjs.cloudflare.com/ajax/libs/highlight.js/9.12.0/highlight.min.js"></script> + <script>hljs.initHighlightingOnLoad();</script> + +</head> + +<body class="wy-body-for-nav" role="document"> + + <div class="wy-grid-for-nav"> + + + <nav data-toggle="wy-nav-shift" class="wy-nav-side stickynav"> + <div class="wy-side-nav-search"> + <a href=".." class="icon icon-home"> PolyMPC</a> + <div role="search"> + <form id ="rtd-search-form" class="wy-form" action="../search.html" method="get"> + <input type="text" name="q" placeholder="Search docs" title="Type search term here" /> + </form> +</div> + </div> + + <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation"> + <ul class="current"> + + + <li class="toctree-l1"> + + <a class="" href="..">Home</a> + </li> + + <li class="toctree-l1"> + + <a class="" href="../build_doc/">Build documentation</a> + </li> + + <li class="toctree-l1 current"> + + <a class="current" href="./">Example</a> + <ul class="subnav"> + + <li class="toctree-l2"><a href="#mobile-robot-example">Mobile Robot example</a></li> + + <ul> + + <li><a class="toctree-l3" href="#model-implementation">Model Implementation</a></li> + + <li><a class="toctree-l3" href="#controller-example">Controller example</a></li> + + </ul> + + + </ul> + </li> + + </ul> + </div> + + </nav> + + <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap"> + + + <nav class="wy-nav-top" role="navigation" aria-label="top navigation"> + <i data-toggle="wy-nav-top" class="fa fa-bars"></i> + <a href="..">PolyMPC</a> + </nav> + + + <div class="wy-nav-content"> + <div class="rst-content"> + <div role="navigation" aria-label="breadcrumbs navigation"> + <ul class="wy-breadcrumbs"> + <li><a href="..">Docs</a> »</li> + + + + <li>Example</li> + <li class="wy-breadcrumbs-aside"> + + </li> + </ul> + <hr/> +</div> + <div role="main"> + <div class="section"> + + <h1 id="mobile-robot-example">Mobile Robot example</h1> +<p>We consider a mobile robot parking problem in a 2D world. +Given an initial position <script type="math/tex">(x,y,\theta)</script> the robot should park at position <script type="math/tex">(0,0,0)</script>, +which is illustrated in the figure below.</p> +<p><img alt="Screenshot" src="../img/mobile_robot.png" /></p> +<p>The robot has control inputs for the forward velocity <script type="math/tex">v</script> and the steering angle <script type="math/tex">\phi</script> +and the wheeldistance <script type="math/tex">d</script> as parameter.</p> +<p>We define the following tricycle kinematic model for this mobile robot: +<script type="math/tex; mode=display"> +\begin{bmatrix} +\dot{x} \\ +\dot{y} \\ +\dot{\theta} +\end{bmatrix}= +\begin{bmatrix} +v \cos(\theta) \cos(\phi) \\ +v \sin(\theta) \cos(\phi) \\ +v \sin(\phi) / d +\end{bmatrix} +</script> +</p> +<h2 id="model-implementation">Model Implementation</h2> +<p>The model can be implemented in the <code>MobileRobot</code> class that defines +the <code>State</code>, <code>Control</code> and <code>Parameters</code> types and the system dynamics function. +The dynamics function needs to be implemented in a templated fashion, +such that it works with Eigen's <code>AutoDiffScalar</code> type for calculating derivatives.</p> +<p>We also need to define the <em>Lagrange</em> and <em>Mayer</em> terms of the cost function. +The <em>Lagrange</em> term defines a cost on the state and control vectors along the prediction horizon +and the <em>Mayer</em> term defines the final cost on the state at the end of the trajectory. +Both terms are quadratic functions, where the matrix coefficients need to be carefully tuned to obtain satisfactory results.</p> +<p>The example implementation is shown in the <code>simple_robot_model.hpp</code> listing:</p> +<pre><code class="Cpp">#ifndef SIMPLE_ROBOT_MODEL_HPP +#define SIMPLE_ROBOT_MODEL_HPP + +#include "Eigen/Dense" + +template <typename _Scalar = double> +struct MobileRobot +{ + MobileRobot(){} + ~MobileRobot(){} + + using Scalar = _Scalar; + using State = Eigen::Matrix<Scalar, 3, 1>; + using Control = Eigen::Matrix<Scalar, 2, 1>; + using Parameters = Eigen::Matrix<Scalar, 1, 1>; + + /** the one for automatic differentiation */ + template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD> + void operator() (const Eigen::MatrixBase<DerivedA> &state, const Eigen::MatrixBase<DerivedB> &control, + const Eigen::MatrixBase<DerivedC> &param, Eigen::MatrixBase<DerivedD> &value) const + { + value[0] = control[0] * cos(state[2]) * cos(control[1]); + value[1] = control[0] * sin(state[2]) * cos(control[1]); + value[2] = control[0] * sin(control[1]) / param[0]; + } +}; + +template<typename _Scalar = double> +struct Lagrange +{ + using Scalar = _Scalar; + using State = Eigen::Matrix<Scalar, 3, 1>; + using Control = Eigen::Matrix<Scalar, 2, 1>; + using Parameters = Eigen::Matrix<Scalar, 1, 1>; + + Eigen::Matrix<Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime> Q; + Eigen::Matrix<Scalar, Control::RowsAtCompileTime, Control::RowsAtCompileTime> R; + + Lagrange(){ + Q << 0.5, 0, 0, + 0, 0.5, 0, + 0, 0, 0.01; + R << 1, 0, + 0, 0.001; + } + ~Lagrange(){} + + + /** the one for automatic differentiation */ + template<typename DerivedA, typename DerivedB, typename DerivedC, typename CostT> + void operator() (const Eigen::MatrixBase<DerivedA> &state, const Eigen::MatrixBase<DerivedB> &control, + const Eigen::MatrixBase<DerivedC> &param, CostT &value) const + { + value = state.dot(Q * state) + control.dot(R * control); + } + +}; + +template<typename _Scalar = double> +struct Mayer +{ + Mayer(){ + Q << 20, 0, 0, + 0, 20, 0, + 0, 0, 10; + } + ~Mayer(){} + + using Scalar = _Scalar; + using State = Eigen::Matrix<Scalar, 3, 1>; + using Control = Eigen::Matrix<Scalar, 2, 1>; + using Parameters = Eigen::Matrix<Scalar, 1, 1>; + + Eigen::Matrix<Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime> Q; + + template<typename StateT, typename CostT> + void operator() (const Eigen::MatrixBase<StateT> &state, CostT &value) const + { + using ScalarT = typename Eigen::MatrixBase<StateT>::Scalar; + value = state.dot(Q.template cast<ScalarT>() * state); + } +}; + +#endif // SIMPLE_ROBOT_MODEL_HPP +</code></pre> + +<h2 id="controller-example">Controller example</h2> +<p>With the model dynamics and cost function defined, +we choose some approximation scheme (here Chebyshev polynomials of order 3) +and construct the NMPC controller. +One needs to choose an initial state, parameters and bounds on state and control vectors.</p> +<p>An code example shown in the <code>main.cpp</code> listing.</p> +<pre><code class="Cpp">#include <iostream> +#include <limits> +#include "control/nmpc.hpp" +#include "control/simple_robot_model.hpp" +#include "polynomials/ebyshev.hpp" +#include "solvers/sqp.hpp" + +using Problem = polympc::OCProblem<MobileRobot<double>, Lagrange<double>, Mayer<double>>; +using Approximation = Chebyshev<3, GAUSS_LOBATTO, double>; +using Controller = polympc::nmpc<Problem, Approximation, sqp::SQP>; + +using State = Controller::State; +using Control = Controller::Control; +using Parameters = Controller::Parameters; + +int main(int argc, char **argv) +{ + Controller robot_controller; + + State x = {-1, -0.5, -0.5}; + Control u; + Parameters p(0.5); + + robot_controller.setStateBounds( + {-10, -10, -std::numeric_limits<double>::infinity()}, + {10, 10, std::numeric_limits<double>::infinity()} + ); + robot_controller.setControlBounds( + {0, -1}, + {1, 1} + ); + robot_controller.setParameters(p); + robot_controller.disableWarmStart(); + + std::cout << "x0 " << x.transpose() << std::endl; + + for (int i = 0; i < 30; i++) { + + robot_controller.computeControl(x); + robot_controller.getOptimalControl(u); + robot_controller.enableWarmStart(); + + // crude integrator + const double dt = 0.001; + for (int j = 0; j < 200; j++) { + State dx; + robot_controller.m_ps_ode.m_f(x, u, p, dx); + x = x + dt * dx; + } + + std::cout << "x " << x.transpose() << " " << + "u " << u.transpose() << std::endl; + + } +} +</code></pre> + + </div> + </div> + <footer> + + <div class="rst-footer-buttons" role="navigation" aria-label="footer navigation"> + + + <a href="../build_doc/" class="btn btn-neutral" title="Build documentation"><span class="icon icon-circle-arrow-left"></span> Previous</a> + + </div> + + + <hr/> + + <div role="contentinfo"> + <!-- Copyright etc --> + + </div> + + Built with <a href="http://www.mkdocs.org">MkDocs</a> using a <a href="https://github.com/snide/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. +</footer> + + </div> + </div> + + </section> + + </div> + + <div class="rst-versions" role="note" style="cursor: pointer"> + <span class="rst-current-version" data-toggle="rst-current-version"> + + + <span><a href="../build_doc/" style="color: #fcfcfc;">« Previous</a></span> + + + </span> +</div> + <script>var base_url = '..';</script> + <script src="../js/theme.js" defer></script> + <script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML" defer></script> + <script src="../docs/mathjaxhelper.js" defer></script> + <script src="../search/main.js" defer></script> + +</body> +</html> diff --git a/img/mobile_robot.png b/img/mobile_robot.png new file mode 100644 index 0000000000000000000000000000000000000000..98fa2bafd27ac1b63a7365669761dcd00461d5fd Binary files /dev/null and b/img/mobile_robot.png differ diff --git a/index.html b/index.html index 72a226a02ea47e7d21a974dd7815a3185fcac261..64bbdc8b08d8905bf163965450e73d1d85604dd3 100644 --- a/index.html +++ b/index.html @@ -64,6 +64,11 @@ <a class="" href="build_doc/">Build documentation</a> </li> + <li class="toctree-l1"> + + <a class="" href="example/">Example</a> + </li> + </ul> </div> @@ -140,6 +145,8 @@ </div> <script>var base_url = '.';</script> <script src="js/theme.js" defer></script> + <script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML" defer></script> + <script src="docs/mathjaxhelper.js" defer></script> <script src="search/main.js" defer></script> </body> @@ -147,5 +154,5 @@ <!-- MkDocs version : 1.0.4 -Build Date UTC : 2019-06-05 09:47:48 +Build Date UTC : 2019-07-02 12:17:22 --> diff --git a/mathjaxhelper.js b/mathjaxhelper.js new file mode 100644 index 0000000000000000000000000000000000000000..08bf8445c3c854bb7898a80e0c0fd3cee5c3a637 --- /dev/null +++ b/mathjaxhelper.js @@ -0,0 +1,8 @@ +MathJax.Hub.Config({ + "tex2jax": { inlineMath: [ [ '$', '$' ] ] } +}); +MathJax.Hub.Config({ + config: ["MMLorHTML.js"], + jax: ["input/TeX", "output/HTML-CSS", "output/NativeMML"], + extensions: ["MathMenu.js", "MathZoom.js"] +}); diff --git a/search.html b/search.html index 2ac3ab85f3b9e650d5d9ec950accddc953b05729..0f88da6106ae342c9b40b7e90a608c61825c7199 100644 --- a/search.html +++ b/search.html @@ -51,6 +51,11 @@ <a class="" href="./build_doc/">Build documentation</a> </li> + <li class="toctree-l1"> + + <a class="" href="./example/">Example</a> + </li> + </ul> </div> @@ -125,6 +130,8 @@ </div> <script>var base_url = '.';</script> <script src="./js/theme.js" defer></script> + <script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML" defer></script> + <script src="./docs/mathjaxhelper.js" defer></script> <script src="./search/main.js" defer></script> </body> diff --git a/search/search_index.json b/search/search_index.json index 4281070ac918362917d069d8af03d55cb38692f5..93c3ab04bea10b5386062a1c50c4a13951702d80 100644 --- a/search/search_index.json +++ b/search/search_index.json @@ -1 +1 @@ -{"config":{"lang":["en"],"prebuild_index":false,"separator":"[\\s\\-]+"},"docs":[{"location":"","text":"PolyMPC PolyMPC: An Efficient and Extensible Tool for Real-Time Nonlinear Model Predictive Tracking and Path Following for Fast Mechatronic Systems. Developed at the Automatic Control laboratory EPFL as part of the AWESCO project.","title":"Home"},{"location":"#polympc","text":"PolyMPC: An Efficient and Extensible Tool for Real-Time Nonlinear Model Predictive Tracking and Path Following for Fast Mechatronic Systems. Developed at the Automatic Control laboratory EPFL as part of the AWESCO project.","title":"PolyMPC"},{"location":"build_doc/","text":"Build documentation using MkDocs This project uses MkDocs for building the documentation site. Installation pip install mkdocs Build and serve locally To build and run the site locally run mkdocs build mkdocs serve Deploy on GitHub Pages WARNING: DO NOT DEPLOY FROM MAIN COPY. DATA CAN BE LOST! First make a copy of the repository from where to build and deploy the documentation. cd polympc-copy mkdocs gh-deploy This will create a branch gh-pages , build the site, commit the result and push to github.","title":"Build documentation"},{"location":"build_doc/#build-documentation-using-mkdocs","text":"This project uses MkDocs for building the documentation site.","title":"Build documentation using MkDocs"},{"location":"build_doc/#installation","text":"pip install mkdocs","title":"Installation"},{"location":"build_doc/#build-and-serve-locally","text":"To build and run the site locally run mkdocs build mkdocs serve","title":"Build and serve locally"},{"location":"build_doc/#deploy-on-github-pages","text":"WARNING: DO NOT DEPLOY FROM MAIN COPY. DATA CAN BE LOST! First make a copy of the repository from where to build and deploy the documentation. cd polympc-copy mkdocs gh-deploy This will create a branch gh-pages , build the site, commit the result and push to github.","title":"Deploy on GitHub Pages"}]} \ No newline at end of file +{"config":{"lang":["en"],"prebuild_index":false,"separator":"[\\s\\-]+"},"docs":[{"location":"","text":"PolyMPC PolyMPC: An Efficient and Extensible Tool for Real-Time Nonlinear Model Predictive Tracking and Path Following for Fast Mechatronic Systems. Developed at the Automatic Control laboratory EPFL as part of the AWESCO project.","title":"Home"},{"location":"#polympc","text":"PolyMPC: An Efficient and Extensible Tool for Real-Time Nonlinear Model Predictive Tracking and Path Following for Fast Mechatronic Systems. Developed at the Automatic Control laboratory EPFL as part of the AWESCO project.","title":"PolyMPC"},{"location":"build_doc/","text":"Build documentation using MkDocs This project uses MkDocs for building the documentation site. Installation pip install mkdocs python-markdown-math Build and serve locally To build and run the site locally run mkdocs build mkdocs serve Deploy on GitHub Pages WARNING: DO NOT DEPLOY FROM MAIN COPY. DATA CAN BE LOST! First make a copy of the repository from where to build and deploy the documentation. cd polympc-copy mkdocs gh-deploy This will create a branch gh-pages , build the site, commit the result and push to github.","title":"Build documentation"},{"location":"build_doc/#build-documentation-using-mkdocs","text":"This project uses MkDocs for building the documentation site.","title":"Build documentation using MkDocs"},{"location":"build_doc/#installation","text":"pip install mkdocs python-markdown-math","title":"Installation"},{"location":"build_doc/#build-and-serve-locally","text":"To build and run the site locally run mkdocs build mkdocs serve","title":"Build and serve locally"},{"location":"build_doc/#deploy-on-github-pages","text":"WARNING: DO NOT DEPLOY FROM MAIN COPY. DATA CAN BE LOST! First make a copy of the repository from where to build and deploy the documentation. cd polympc-copy mkdocs gh-deploy This will create a branch gh-pages , build the site, commit the result and push to github.","title":"Deploy on GitHub Pages"},{"location":"example/","text":"Mobile Robot example We consider a mobile robot parking problem in a 2D world. Given an initial position (x,y,\\theta) the robot should park at position (0,0,0) , which is illustrated in the figure below. The robot has control inputs for the forward velocity v and the steering angle \\phi and the wheeldistance d as parameter. We define the following tricycle kinematic model for this mobile robot: \\begin{bmatrix} \\dot{x} \\\\ \\dot{y} \\\\ \\dot{\\theta} \\end{bmatrix}= \\begin{bmatrix} v \\cos(\\theta) \\cos(\\phi) \\\\ v \\sin(\\theta) \\cos(\\phi) \\\\ v \\sin(\\phi) / d \\end{bmatrix} Model Implementation The model can be implemented in the MobileRobot class that defines the State , Control and Parameters types and the system dynamics function. The dynamics function needs to be implemented in a templated fashion, such that it works with Eigen's AutoDiffScalar type for calculating derivatives. We also need to define the Lagrange and Mayer terms of the cost function. The Lagrange term defines a cost on the state and control vectors along the prediction horizon and the Mayer term defines the final cost on the state at the end of the trajectory. Both terms are quadratic functions, where the matrix coefficients need to be carefully tuned to obtain satisfactory results. The example implementation is shown in the simple_robot_model.hpp listing: #ifndef SIMPLE_ROBOT_MODEL_HPP #define SIMPLE_ROBOT_MODEL_HPP #include Eigen/Dense template typename _Scalar = double struct MobileRobot { MobileRobot(){} ~MobileRobot(){} using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; /** the one for automatic differentiation */ template typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD void operator() (const Eigen::MatrixBase DerivedA state, const Eigen::MatrixBase DerivedB control, const Eigen::MatrixBase DerivedC param, Eigen::MatrixBase DerivedD value) const { value[0] = control[0] * cos(state[2]) * cos(control[1]); value[1] = control[0] * sin(state[2]) * cos(control[1]); value[2] = control[0] * sin(control[1]) / param[0]; } }; template typename _Scalar = double struct Lagrange { using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; Eigen::Matrix Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime Q; Eigen::Matrix Scalar, Control::RowsAtCompileTime, Control::RowsAtCompileTime R; Lagrange(){ Q 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.01; R 1, 0, 0, 0.001; } ~Lagrange(){} /** the one for automatic differentiation */ template typename DerivedA, typename DerivedB, typename DerivedC, typename CostT void operator() (const Eigen::MatrixBase DerivedA state, const Eigen::MatrixBase DerivedB control, const Eigen::MatrixBase DerivedC param, CostT value) const { value = state.dot(Q * state) + control.dot(R * control); } }; template typename _Scalar = double struct Mayer { Mayer(){ Q 20, 0, 0, 0, 20, 0, 0, 0, 10; } ~Mayer(){} using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; Eigen::Matrix Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime Q; template typename StateT, typename CostT void operator() (const Eigen::MatrixBase StateT state, CostT value) const { using ScalarT = typename Eigen::MatrixBase StateT ::Scalar; value = state.dot(Q.template cast ScalarT () * state); } }; #endif // SIMPLE_ROBOT_MODEL_HPP Controller example With the model dynamics and cost function defined, we choose some approximation scheme (here Chebyshev polynomials of order 3) and construct the NMPC controller. One needs to choose an initial state, parameters and bounds on state and control vectors. An code example shown in the main.cpp listing. #include iostream #include limits #include control/nmpc.hpp #include control/simple_robot_model.hpp #include polynomials/ebyshev.hpp #include solvers/sqp.hpp using Problem = polympc::OCProblem MobileRobot double , Lagrange double , Mayer double ; using Approximation = Chebyshev 3, GAUSS_LOBATTO, double ; using Controller = polympc::nmpc Problem, Approximation, sqp::SQP ; using State = Controller::State; using Control = Controller::Control; using Parameters = Controller::Parameters; int main(int argc, char **argv) { Controller robot_controller; State x = {-1, -0.5, -0.5}; Control u; Parameters p(0.5); robot_controller.setStateBounds( {-10, -10, -std::numeric_limits double ::infinity()}, {10, 10, std::numeric_limits double ::infinity()} ); robot_controller.setControlBounds( {0, -1}, {1, 1} ); robot_controller.setParameters(p); robot_controller.disableWarmStart(); std::cout x0 x.transpose() std::endl; for (int i = 0; i 30; i++) { robot_controller.computeControl(x); robot_controller.getOptimalControl(u); robot_controller.enableWarmStart(); // crude integrator const double dt = 0.001; for (int j = 0; j 200; j++) { State dx; robot_controller.m_ps_ode.m_f(x, u, p, dx); x = x + dt * dx; } std::cout x x.transpose() u u.transpose() std::endl; } }","title":"Example"},{"location":"example/#mobile-robot-example","text":"We consider a mobile robot parking problem in a 2D world. Given an initial position (x,y,\\theta) the robot should park at position (0,0,0) , which is illustrated in the figure below. The robot has control inputs for the forward velocity v and the steering angle \\phi and the wheeldistance d as parameter. We define the following tricycle kinematic model for this mobile robot: \\begin{bmatrix} \\dot{x} \\\\ \\dot{y} \\\\ \\dot{\\theta} \\end{bmatrix}= \\begin{bmatrix} v \\cos(\\theta) \\cos(\\phi) \\\\ v \\sin(\\theta) \\cos(\\phi) \\\\ v \\sin(\\phi) / d \\end{bmatrix}","title":"Mobile Robot example"},{"location":"example/#model-implementation","text":"The model can be implemented in the MobileRobot class that defines the State , Control and Parameters types and the system dynamics function. The dynamics function needs to be implemented in a templated fashion, such that it works with Eigen's AutoDiffScalar type for calculating derivatives. We also need to define the Lagrange and Mayer terms of the cost function. The Lagrange term defines a cost on the state and control vectors along the prediction horizon and the Mayer term defines the final cost on the state at the end of the trajectory. Both terms are quadratic functions, where the matrix coefficients need to be carefully tuned to obtain satisfactory results. The example implementation is shown in the simple_robot_model.hpp listing: #ifndef SIMPLE_ROBOT_MODEL_HPP #define SIMPLE_ROBOT_MODEL_HPP #include Eigen/Dense template typename _Scalar = double struct MobileRobot { MobileRobot(){} ~MobileRobot(){} using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; /** the one for automatic differentiation */ template typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD void operator() (const Eigen::MatrixBase DerivedA state, const Eigen::MatrixBase DerivedB control, const Eigen::MatrixBase DerivedC param, Eigen::MatrixBase DerivedD value) const { value[0] = control[0] * cos(state[2]) * cos(control[1]); value[1] = control[0] * sin(state[2]) * cos(control[1]); value[2] = control[0] * sin(control[1]) / param[0]; } }; template typename _Scalar = double struct Lagrange { using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; Eigen::Matrix Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime Q; Eigen::Matrix Scalar, Control::RowsAtCompileTime, Control::RowsAtCompileTime R; Lagrange(){ Q 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.01; R 1, 0, 0, 0.001; } ~Lagrange(){} /** the one for automatic differentiation */ template typename DerivedA, typename DerivedB, typename DerivedC, typename CostT void operator() (const Eigen::MatrixBase DerivedA state, const Eigen::MatrixBase DerivedB control, const Eigen::MatrixBase DerivedC param, CostT value) const { value = state.dot(Q * state) + control.dot(R * control); } }; template typename _Scalar = double struct Mayer { Mayer(){ Q 20, 0, 0, 0, 20, 0, 0, 0, 10; } ~Mayer(){} using Scalar = _Scalar; using State = Eigen::Matrix Scalar, 3, 1 ; using Control = Eigen::Matrix Scalar, 2, 1 ; using Parameters = Eigen::Matrix Scalar, 1, 1 ; Eigen::Matrix Scalar, State::RowsAtCompileTime, State::RowsAtCompileTime Q; template typename StateT, typename CostT void operator() (const Eigen::MatrixBase StateT state, CostT value) const { using ScalarT = typename Eigen::MatrixBase StateT ::Scalar; value = state.dot(Q.template cast ScalarT () * state); } }; #endif // SIMPLE_ROBOT_MODEL_HPP","title":"Model Implementation"},{"location":"example/#controller-example","text":"With the model dynamics and cost function defined, we choose some approximation scheme (here Chebyshev polynomials of order 3) and construct the NMPC controller. One needs to choose an initial state, parameters and bounds on state and control vectors. An code example shown in the main.cpp listing. #include iostream #include limits #include control/nmpc.hpp #include control/simple_robot_model.hpp #include polynomials/ebyshev.hpp #include solvers/sqp.hpp using Problem = polympc::OCProblem MobileRobot double , Lagrange double , Mayer double ; using Approximation = Chebyshev 3, GAUSS_LOBATTO, double ; using Controller = polympc::nmpc Problem, Approximation, sqp::SQP ; using State = Controller::State; using Control = Controller::Control; using Parameters = Controller::Parameters; int main(int argc, char **argv) { Controller robot_controller; State x = {-1, -0.5, -0.5}; Control u; Parameters p(0.5); robot_controller.setStateBounds( {-10, -10, -std::numeric_limits double ::infinity()}, {10, 10, std::numeric_limits double ::infinity()} ); robot_controller.setControlBounds( {0, -1}, {1, 1} ); robot_controller.setParameters(p); robot_controller.disableWarmStart(); std::cout x0 x.transpose() std::endl; for (int i = 0; i 30; i++) { robot_controller.computeControl(x); robot_controller.getOptimalControl(u); robot_controller.enableWarmStart(); // crude integrator const double dt = 0.001; for (int j = 0; j 200; j++) { State dx; robot_controller.m_ps_ode.m_f(x, u, p, dx); x = x + dt * dx; } std::cout x x.transpose() u u.transpose() std::endl; } }","title":"Controller example"}]} \ No newline at end of file diff --git a/sitemap.xml b/sitemap.xml index 16e949998f5903b170d8f248072348a37172bb47..6d8bfffc0f267b12cd9f7bbd93819be4a7563bc4 100644 --- a/sitemap.xml +++ b/sitemap.xml @@ -2,12 +2,17 @@ <urlset xmlns="http://www.sitemaps.org/schemas/sitemap/0.9"> <url> <loc>None</loc> - <lastmod>2019-06-05</lastmod> + <lastmod>2019-07-02</lastmod> <changefreq>daily</changefreq> </url> <url> <loc>None</loc> - <lastmod>2019-06-05</lastmod> + <lastmod>2019-07-02</lastmod> + <changefreq>daily</changefreq> + </url> + <url> + <loc>None</loc> + <lastmod>2019-07-02</lastmod> <changefreq>daily</changefreq> </url> </urlset> \ No newline at end of file diff --git a/sitemap.xml.gz b/sitemap.xml.gz index 21f9b2bf08806a87d61bb7ba0abf471d0003aeff..d3a76ce54088233befba61b64817f62df3b39fac 100644 Binary files a/sitemap.xml.gz and b/sitemap.xml.gz differ