{ "cells": [ { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "# **Welcome to Crocoddyl**\n", "\n", "Crocoddyl is an **optimal control library** designed for **robot control in contact-rich environments**. It leverages an efficient **Differential Dynamic Programming (DDP)** solver to compute **optimal trajectories** and **feedback gains** for complex robotic systems. \n", "\n", "To achieve high-performance control, Crocoddyl integrates **Pinocchio**, a state-of-the-art rigid body dynamics library, enabling fast computation of **robot dynamics and analytical derivatives**. \n", "\n", "### Multi-Contact Optimal Control Problem (MCOP)\n", "\n", "Crocoddyl is specifically designed to handle **multi-contact optimal control problems (MCOPs)**, which take the following form:\n", "\n", "\n", "\\begin{aligned}\n", "\\mathbf{X}^*, \\mathbf{U}^* =\n", "\\begin{Bmatrix} \\mathbf{x}^*_0, \\dots, \\mathbf{x}^*_N \\\\\n", " \\mathbf{u}^*_0, \\dots, \\mathbf{u}^*_N\n", "\\end{Bmatrix} &=\n", "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{k=1}^{N} \\int_{t_k}^{t_k+\\Delta t} \\ell(\\mathbf{x}, \\mathbf{u})dt \\\\\n", "\n", "&\\text{subject to} \\\\\n", "\n", "&\\mathbf{\\dot{x}} = \\mathbf{f}(\\mathbf{x}, \\mathbf{u}),\\\\\n", "\n", "&\\mathbf{h}(\\mathbf{x}, \\mathbf{u}) = \\mathbf{0}, \\\\\n", "\n", "&\\mathbf{\\underline{g}} \\leq \\mathbf{g}(\\mathbf{x}, \\mathbf{u}) \\leq \\mathbf{\\overline{g}}, \\\\\n", "\n", "&\\mathbf{x} \\in \\mathcal{X}, \\quad \\mathbf{u} \\in \\mathcal{U}.\n", "\\end{aligned}\n", "\n", "where: \n", "- The **state** is given by $\\mathbf{x} = (\\mathbf{q}, \\mathbf{v})$, which lies on a **Lie manifold** (e.g., $\\mathbf{q} \\in SE(3) \\times \\mathbb{R}^{n_j}$), where $n_j$ is the number of degrees of freedom of the robot. \n", "- The system exhibits **underactuated dynamics**, meaning the control input takes the form $\\mathbf{u} = (\\mathbf{0}, \\boldsymbol{\\tau})$. \n", "- $\\mathcal{X}$ and $\\mathcal{U}$ define the **state and control admissible sets**. \n", "- $\\mathcal{K}$ represents the **contact constraints**. \n", "\n", "## Getting Started\n", "\n", "To fully understand Crocoddyl’s **design and capabilities**, let's begin by exploring its fundamental concepts and framework. 🚀\n", "\n", "\n", "### I. Action Models in Crocoddyl\n", "\n", "In Crocoddyl, an **action model** encapsulates both **dynamics** and **cost models**. Each **node** in an optimal control problem is described using an action model. To formulate an optimal control problem, we must define:\n", "- The **dynamics function**, which models system evolution.\n", "- The **cost function**, which quantifies control objectives.\n", "- Their **derivatives**, which enable efficient optimization.\n", "\n", "To understand the mathematical foundation of an action model, we consider a **locally linearized** version of our optimal control problem:\n", "\n", "$$\\mathbf{X}^*(\\mathbf{x}_0),\\mathbf{U}^*(\\mathbf{x}_0)\n", "=\n", "\\arg\\max_{\\mathbf{X},\\mathbf{U}} = cost_T(\\delta\\mathbf{x}_N) + \\sum_{k=1}^N cost_t(\\delta\\mathbf{x}_k, \\delta\\mathbf{u}_k)$$\n", "subject to\n", "$$dynamics(\\delta\\mathbf{x}_{k+1},\\delta\\mathbf{x}_k,\\delta\\mathbf{u}_k)=\\mathbf{0},$$\n", "\n", "where\n", "$$cost_T(\\delta\\mathbf{x}) = \\frac{1}{2}\n", "\\begin{bmatrix} \n", " 1 \\\\ \\delta\\mathbf{x}\n", "\\end{bmatrix}^\\top\n", "\\begin{bmatrix}\n", "0 & \\mathbf{\\ell_x}^\\top \\\\\n", "\\mathbf{\\ell_x} & \\mathbf{\\ell_{xx}}\n", "\\end{bmatrix}\n", "\\begin{bmatrix}\n", " 1 \\\\ \\delta\\mathbf{x}\n", "\\end{bmatrix}\n", "$$\n", "\n", "$$cost_t(\\delta\\mathbf{x},\\delta\\mathbf{u}) = \\frac{1}{2}\n", "\\begin{bmatrix} \n", " 1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n", "\\end{bmatrix}^\\top\n", "\\begin{bmatrix}\n", "0 & \\mathbf{\\ell_x}^\\top & \\mathbf{l_u}^\\top\\\\\n", "\\mathbf{\\ell_x} & \\mathbf{\\ell_{xx}} & \\mathbf{\\ell_{ux}}^\\top\\\\\n", "\\mathbf{\\ell_u} & \\mathbf{\\ell_{ux}} & \\mathbf{\\ell_{uu}}\n", "\\end{bmatrix}\n", "\\begin{bmatrix}\n", " 1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n", "\\end{bmatrix}\n", "$$\n", "\n", "$$\n", "dynamics(\\delta\\mathbf{x}_{k+1},\\delta\\mathbf{x}_k,\\delta\\mathbf{u}_k) = \\delta\\mathbf{x}_{k+1} - (\\mathbf{f_x}\\delta\\mathbf{x}_k + \\mathbf{f_u}\\delta\\mathbf{u}_k)\n", "$$\n", "\n", "where an action model defines a time interval of this problem:\n", " - $actions = dynamics + cost$\n", "\n", "### Key Properties of Action Models\n", "- **Encapsulates system dynamics and cost functions** for an optimal control problem node.\n", "- **Operates in discrete-time** (unlike continuous formulations).\n", "- **Provides analytical derivatives** for efficient optimization, leveraging **Pinocchio** for computational efficiency.\n", "- **Supports numerical/automati differentiation (NumDiff/AutoDiff) and code generation (CodeGen)** for debugging, prototyping, and deploying.\n", "\n", "### Main Computation in an Action Model\n", "\n", "Optimal control solvers often need to compute a search direction and try it along a step length. These computations are performed by Crocoddyl's `calc` and `calcDiff` functions. Concretely, these functions update the information of:\n", " - **`calc`**: update the next state, cost value, and constraint infeasibilities.\n", " $$\\delta\\mathbf{\\dot{x}}_{k+1} = \\mathbf{f}(\\delta\\mathbf{x}_k,\\mathbf{u}_k)$$\n", " - **`calcDiff`**: update the derivatives of the dynamics, cost, and constraints (quadratic approximation)\n", " $$\\mathbf{f_x}, \\mathbf{f_u} \\hspace{1em} (dynamics)$$\n", " $$\\mathbf{\\ell_x}, \\mathbf{\\ell_u}, \\mathbf{\\ell_{xx}}, \\mathbf{\\ell_{ux}}, \\mathbf{\\ell_{uu}} \\hspace{1em} (cost)$$\n", " \n", "To optimize the performance of the solver, Crocoddyl stores all relevant information inside **data** objects. This approach avoids dynamic memory reallocation during the calculations, which helps to speed up the process, especially when the solver is called iteratively in the optimization process. Creating each data object is done via `createData`.\n", "\n", "With this understanding, we can now explore **how to implement and use action models in Crocoddyl** in `02_optimizing_a_cartpole.ipynb`.\n", "\n", "---\n", "\n", "### II. States in Crocoddyl\n", "\n", "\n", "In general, the system's state lies in a manifold $M$, where the rate of change of the state lies in its tangent space $T_\\mathbf{x}M$. To perform operations within our solvers, we define the following operators:\n", "\n", "- **Integration**: The operation to compute the next state given the current state and the change in state:\n", " \n", " \\begin{equation}\n", " \\mathbf{x}_{k+1} = \\text{integrate}(\\mathbf{x}_k, \\delta \\mathbf{x}_k) = \\mathbf{x}_k \\oplus \\delta \\mathbf{x}_k\n", " \\end{equation}\n", " \n", "- **Difference**: The operation to compute the change in state given two consecutive states:\n", " \n", " \\begin{equation}\n", " \\delta \\mathbf{x}_k = \\text{difference}(\\mathbf{x}_{k+1}, \\mathbf{x}_k) = \\mathbf{x}_{k+1} \\ominus \\mathbf{x}_k\n", " \\end{equation}\n", "\n", "where $\\mathbf{x} \\in M$ (the manifold) and $\\delta \\mathbf{x} \\in T_\\mathbf{x} M$ (the tangent space at $\\mathbf{x}$).\n", "\n", "To compute the necessary derivatives during optimization, we also define the Jacobians of these operators with respect to their arguments:\n", "\n", "- The Jacobian of the **integration** operator with respect to the first and second arguments:\n", " \n", " \\begin{equation}\n", " \\frac{\\partial (\\mathbf{x} \\oplus \\delta \\mathbf{x})}{\\partial \\mathbf{x}}, \\frac{\\partial (\\mathbf{x} \\oplus \\delta \\mathbf{x})}{\\partial \\delta \\mathbf{x}} = J_{\\text{integrate}}(\\mathbf{x}, \\delta \\mathbf{x})\n", " \\end{equation}\n", "\n", "- The Jacobian of the **difference** operator with respect to the first and second arguments:\n", "\n", " \\begin{equation}\n", " \\frac{\\partial (\\mathbf{x}_2 \\ominus \\mathbf{x}_1)}{\\partial \\mathbf{x}_2}, \\frac{\\partial (\\mathbf{x}_2 \\ominus \\mathbf{x}_1)}{\\partial \\mathbf{x}_1} = J_{\\text{difference}}(\\mathbf{x}_2, \\mathbf{x}_1)\n", " \\end{equation}\n", "\n", " With this understanding, we can now explore **how to implement a state class in Crocoddyl** in `03_optimizing_an_unicycle.ipynb`.\n", "\n", "---\n", "\n", "### III. Actuation Models in Crocoddyl\n", "\n", "Work in progress. More details in `04_actuating_an_acrobot.ipynb`.\n", "\n", "---\n", "\n", "### IV. Activation and Residual Models in Crocoddyl\n", "\n", "Work in progress. More details in `07_targetting_in_aerial_manipulation.ipynb`.\n", "\n", "---\n", "\n", "### V. Inverse vs Forward Dynamics\n", "\n", "Work in progress. More details in `08_backflips_in_humanoids.ipynb`." ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "## Reference\n", "\n", "The material presented in this Notebook was previously presented at the ICRA at 2020. For more information, please read the following paper:\n", "\n", "C. Mastalli et al. Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control, 2020" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from IPython.display import HTML\n", "\n", "# Youtube\n", "HTML(\n", " ''\n", ")" ] }, { "attachments": {}, "cell_type": "markdown", "metadata": {}, "source": [ "\n", "\n", "![title](https://cmastalli.github.io/assets/img/publications/astronaut360_v2.png)" ] } ], "metadata": { "celltoolbar": "Attachments", "kernelspec": { "display_name": "crocoddyl", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.12.9" } }, "nbformat": 4, "nbformat_minor": 2 }