{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# The HFM library - A fast marching solver with adaptive stencils\n", "\n", "## Part : Non holonomic metrics and curvature penalization\n", "## Chapter : Vehicles with trailers" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We compute globally minimal paths for an omni-directional vehicle equipped with one or two trailers.\n", "For that purpose, we numerically solve sub-Riemannian eikonal equations in dimension three and four. The numerical scheme is GPU accelerated, with features aimed to reduce numerical diffusion. The extracted trajectories are validated by comparison with a geodesic shooting method.\n", "\n", "**References.**\n", "\n", "The numerical scheme used in this notebook is described and studied in detail in the following publication:\n", "* Mirebeau, J.-M. (2019). Riemannian Fast-Marching on Cartesian Grids, Using Voronoi's First Reduction of Quadratic Forms. SIAM Journal on Numerical Analysis, 57(6), 2608–2655." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "[**Summary**](Summary.ipynb) of volume Fast Marching Methods, this series of notebooks.\n", "\n", "[**Main summary**](../Summary.ipynb) of the Adaptive Grid Discretizations \n", "\tbook of notebooks, including the other volumes.\n", "\n", "# Table of contents\n", " * [1. Mathematical framework](#1.-Mathematical-framework)\n", " * [1.1 The vehicle models](#1.1-The-vehicle-models)\n", " * [1.2 Properties of the models](#1.2-Properties-of-the-models)\n", " * [1.3 Sub-Riemannian formalism](#1.3-Sub-Riemannian-formalism)\n", " * [1.4 Numerical implementation](#1.4-Numerical-implementation)\n", " * [2. One trailer](#2.-One-trailer)\n", " * [2.1 Control vector fields](#2.1-Control-vector-fields)\n", " * [2.2 Input](#2.2-Input)\n", " * [2.3 Special motions](#2.3-Special-motions)\n", " * [2.4 Comparison with the Reeds-Shepp model](#2.4-Comparison-with-the-Reeds-Shepp-model)\n", " * [3. Two trailers](#3.-Two-trailers)\n", " * [3.1 Control vector fields](#3.1-Control-vector-fields)\n", " * [3.2 Input](#3.2-Input)\n", " * [3.3 Special motions](#3.3-Special-motions)\n", " * [3.4 Comparison with the Reeds-Shepp car](#3.4-Comparison-with-the-Reeds-Shepp-car)\n", "\n", "\n", "\n", "This Python® notebook is intended as documentation and testing for the [HamiltonFastMarching (HFM) library](https://github.com/mirebeau/HamiltonFastMarching), which also has interfaces to the Matlab® and Mathematica® languages. \n", "More information on the HFM library in the manuscript:\n", "* Jean-Marie Mirebeau, Jorg Portegies, \"Hamiltonian Fast Marching: A numerical solver for anisotropic and non-holonomic eikonal PDEs\", 2019 [(link)](https://hal.archives-ouvertes.fr/hal-01778322)\n", "\n", "Copyright Jean-Marie Mirebeau, University Paris-Sud, CNRS, University Paris-Saclay" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 0. Importing the required libraries" ] }, { "cell_type": "code", "execution_count": 1, "metadata": { "execution": { "iopub.execute_input": "2024-04-30T08:58:37.569590Z", "iopub.status.busy": "2024-04-30T08:58:37.568953Z", "iopub.status.idle": "2024-04-30T08:58:37.578253Z", "shell.execute_reply": "2024-04-30T08:58:37.577706Z" } }, "outputs": [], "source": [ "import sys; sys.path.insert(0,\"..\")\n", "#from Miscellaneous import TocTools; print(TocTools.displayTOC('Trailers','FMM'))" ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "execution": { "iopub.execute_input": "2024-04-30T08:58:37.581184Z", "iopub.status.busy": "2024-04-30T08:58:37.580957Z", "iopub.status.idle": "2024-04-30T08:58:37.686492Z", "shell.execute_reply": "2024-04-30T08:58:37.686211Z" } }, "outputs": [], "source": [ "from agd import Eikonal\n", "from agd import AutomaticDifferentiation as ad\n", "from agd import Metrics\n", "from agd import LinearParallel as lp\n", "norm_infinity = ad.Optimization.norm_infinity" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "execution": { "iopub.execute_input": "2024-04-30T08:58:37.688326Z", "iopub.status.busy": "2024-04-30T08:58:37.688205Z", "iopub.status.idle": "2024-04-30T08:58:37.843081Z", "shell.execute_reply": "2024-04-30T08:58:37.842706Z" } }, "outputs": [], "source": [ "import numpy as np; xp=np; allclose = np.allclose\n", "from matplotlib import pyplot as plt\n", "np.set_printoptions(edgeitems=30, linewidth=100000, formatter=dict(float=lambda x: \"%5.3g\" % x))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 0.1 Additional configuration\n", "\n", "The experiments presented in this notebook require the GPU accelerated solver. \n", "(Adapting them for the cpu solver would not be too difficult, although computation times would be longer.)" ] }, { "cell_type": "code", "execution_count": 4, "metadata": { "execution": { "iopub.execute_input": "2024-04-30T08:58:37.844824Z", "iopub.status.busy": "2024-04-30T08:58:37.844702Z", "iopub.status.idle": "2024-04-30T08:58:37.935973Z", "shell.execute_reply": "2024-04-30T08:58:37.935533Z" } }, "outputs": [ { "ename": "DeliberateNotebookError", "evalue": "cupy module not found", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mDeliberateNotebookError\u001b[0m Traceback (most recent call last)", "Cell \u001b[0;32mIn[4], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m ad\u001b[38;5;241m.\u001b[39mcupy_generic\u001b[38;5;241m.\u001b[39mcp \u001b[38;5;129;01mis\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m: \u001b[38;5;28;01mraise\u001b[39;00m ad\u001b[38;5;241m.\u001b[39mDeliberateNotebookError(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mcupy module not found\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", "\u001b[0;31mDeliberateNotebookError\u001b[0m: cupy module not found" ] } ], "source": [ "if ad.cupy_generic.cp is None: raise ad.DeliberateNotebookError(\"cupy module not found\")" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Replacing numpy with cupy, set to output 32bit ints and floats by default.\n", "Using cp.asarray(*,dtype=np.float32) as the default caster in ad.array.\n", "Setting dictIn.default_mode = 'gpu' in module agd.Eikonal .\n", "Returning a copy of module matplotlib.pyplot whose functions accept cupy arrays as input.\n", "Setting float32 compatible default values atol=rtol=1e-5 in np.allclose\n" ] } ], "source": [ "xp,Eikonal,plt,allclose = [ad.cupy_friendly(e) for e in (xp,Eikonal,plt,allclose)]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 1. Mathematical framework\n", "\n", "The goal of this section is to describe the models of interest in terms of controls $(v_i)$, and complementary controls $(w_i)$ (associated to forbidden motions) whose efficiency is determined by a small relaxation parameter $\\epsilon > 0$. The resulting quadratic hamiltonian if defined as \n", "$$\n", " H(q,p) = \\frac 1 2 \\Big (\\sum_i ^2 + \\epsilon^2 \\sum_i ^2\\Big).\n", "$$" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [], "source": [ "def Hamiltonian(controls,complement,ϵ=0.1):\n", " \"\"\"\n", " The quadratic Hamiltonian associated to a family of control vector fields, \n", " and relaxed with a family of complementary vector fields.\n", " \"\"\"\n", " return lp.outer_self(controls).sum(axis=2) + lp.outer_self(ϵ*complement).sum(axis=2) " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 1.1 The vehicle models\n", "\n", "**Omni-directional vehicle.** Our starting point is a vehicle able to move in any direction in the plane, at unit Euclidean speed. Denote by $X \\in R^2$ its position. In control theoretic terms, the evolution of $X$ is expressed in an orthonormal basis $(e,e^\\perp)$ of the plane, and determined by two controls $(\\alpha, \\beta)$ in the unit ball.\n", "$$\n", " \\dot X = \\alpha e + \\beta e^\\perp, \\qquad \\alpha^2 + \\beta^2 \\leq 1.\n", "$$\n", "\n", "\n", "**Case of one trailer.** Let us attach a trailer to the preceding vehicle. The trailer is rigid and has fixed length $r>0$, also denoted $r=1/\\kappa$ where $\\kappa$ can be regarded as a typical curvature. The full vehicle state is now $(X,\\theta) \\in R^2 \\times S^1$, where $e(\\theta) := (\\cos \\theta , \\sin \\theta)$ is colinear to the trailer direction and directed toward the (omni-directional) head. The trailer state cannot be controlled directly, thus the evolution of $(X,\\theta)$ is again determined by two controls $(\\alpha,\\beta)$ in the unit ball and reads \n", "$$\n", " \\begin{pmatrix}\n", " \\dot X\\\\\n", " \\dot \\theta\n", " \\end{pmatrix}\n", " {=}\n", " \\alpha \n", " \\begin{pmatrix}\n", " e(\\theta)\\\\\n", " 0\n", " \\end{pmatrix}\n", " {+}\n", " \\beta\n", " \\begin{pmatrix}\n", " e(\\theta)^\\perp\\\\\n", " \\kappa\n", " \\end{pmatrix}.\n", "$$\n", "Note that we use a block notation for vectors; for instance $(X,\\theta) := (X_0,X_1,\\theta)$ if $X = (X_0,X_1) \\in R^2$.\n", "A few remarks are in order:\n", "- The head $X$ of the vehicle follows the same dynamics as in the previous simpler model, only expressed in the peculiar orthonormal basis $e((\\theta),e(\\theta)^\\perp)$. Again, the head can always move in any direction, at unit Euclidean speed.\n", "- If the head moves in the direction $e(\\theta)$, aligned with the trailer direction, then the latter is only trailing behing and its orientation does not change, as expected.\n", "- A special motion of interest is when the head moves in circles of radius $r = 1/\\kappa$ in the direction $e(\\theta)^\\perp$, so that the rear wheels of the trailer do not move, although its angle $\\theta$ changes continuously. The second control vector corresponds, at first order, to this motion.\n", "\n", "**Case of two trailers.**\n", "Let us attach a second trailer at to the preceding vehicle, at the back of the first trailer. The additional trailer is again rigid, and has fixed length $r'>0$, also denoted $r' = 1/\\kappa'$. The angle of the second trailer is again denoted $\\phi$. The vehicle state $(X,\\theta,\\phi) \\in R^2 \\times S^1 \\times S^1$ is now four dimensional, but still only two controls $(\\alpha, \\beta)$ are available. The evolution equation reads:\n", "$$\n", " \\begin{pmatrix}\n", " \\dot X \\\\\n", " \\dot \\theta \\\\\n", " \\dot \\phi\n", " \\end{pmatrix}\n", " {=}\n", " \\alpha \n", " \\begin{pmatrix}\n", " e(\\theta) \\\\\n", " 0 \\\\\n", " \\kappa' \\sin(\\theta-\\phi)\n", " \\end{pmatrix}\n", " {+}\n", " \\beta\n", " \\begin{pmatrix}\n", " e(\\theta)^\\perp \\\\\n", " \\kappa \\\\\n", " 0\n", " \\end{pmatrix}\n", "$$\n", "- Control $\\alpha$ corresponds to the vehicle moving forward, so that the first trailer orientation is not affected, but the second trailer tends to align with the first one.\n", "- Control $\\beta$ corresponds to the omni-directional head moving into circles of radius $r=1/\\kappa$, leaving in place the rear wheels of the first trailer, as described above. The second trailer is unaffected by this motion." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 1.2 Properties of the models\n", "\n", "**Commutators.**\n", "Denote by $\\omega_0$ and $\\omega_1$ the two control vector fields of (one of our three) models. In the trailer-less case, $\\omega_0$ and $\\omega_1$ span the plane $R^2$. In other words, any tangent vector to the state space $R^2$ can be realized by a suitable linear combination $\\alpha\\omega_0+ \\beta \\omega_1$.\n", "\n", "As soon as one trailer is attached, the two vectors vector fields $\\omega_0,\\omega_1$ fail to span the tangent space to the state space $R^2 \\times S^1$, which is three dimensional. The model is said non-holonomic. Alternating small (first-order) displacements along $\\omega_0$ and $\\omega_1$ one may however realize a motion (second-order small) in a direction $\\omega_2$ which is not a linear combination of $\\omega_0$ and $\\omega_1$, namely here $\\omega_2 = (e(\\theta)^\\perp,0)$. One says that $\\omega_2$ is the commutor of $\\omega_0$ and $\\omega_1$.\n", "\n", "When two trailers are attached, the two vector fields $\\omega_0, \\omega_1$ and their first order commutator $\\omega_2$ still do not span the tangent space to the state space $R^2 \\times S^1 \\times S^1$, which is four dimensional. Yet another commutator, of $\\omega_2$ with $\\omega_0$ or $\\omega_1$ is required for that purpose, and one says that the model is sub-Riemannian of rank two.\n", "\n", "\n", "**Comparison with the Reeds-Shepp model.**\n", "\n", "To (some) mathematicians, wether the dog is wagging the tail, or the tail is wagging the dog, is only a question of coordinate system choice. Following that rationale, and we denote by $Y = X - r e(\\theta)$ the position of the tail of the trailer, in the single trailer case. One easily checks that \n", "$$\n", " \\begin{pmatrix}\n", " \\dot Y \\\\\n", " \\dot \\theta \n", " \\end{pmatrix}\n", " {=} \n", " \\alpha\n", " \\begin{pmatrix}\n", " e(\\theta) \\\\\n", " 0\n", " \\end{pmatrix}\n", " {+}\n", " \\beta\n", " \\begin{pmatrix}\n", " 0 \\\\\n", " \\kappa\n", " \\end{pmatrix}.\n", "$$\n", "This particular optimal control model is known as the Reeds-Shepp car, and best represents a wheelchair-like vehicle. Up to a coordinate change, it is thus isomorphic to the omni-directional vehicle with a single trailer. \n", "(Implementation-wise, one notable difference is that the the Reeds-Shepp hamiltonian is block diagonal with sizes (2,1), in contrast with the original model which has no such structure.)\n", "\n", "The omni-directional vehicle with two trailers is, likewise, isomorphic to a Reeds-Shepp car with a single trailer." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 1.3 Sub-Riemannian formalism\n", "\n", "**Optimal control.** Denote by $\\Omega$ a sub-domain of the state space (for our models $R^2$, $R^2 \\times S^1$ or $R^2 \\times S^1 \\times S^1$). \n", "Denote by $\\omega_0, \\cdots, \\omega_n$ the control vector fields (for our models $n=1$), and by $\\alpha_0, \\cdots, \\alpha_n$ the related controls (for our models denoted $\\alpha, \\beta$).\n", "\n", "Our objective is to evaluate the minimal cost $U(z)$ of a path from a reference seed point $z_0$ to a given point $z$. More precisely, \n", "$$\n", " U(z)^2 = \\min_{\\alpha_0,\\cdots,\\alpha_n}\\int_0^1 (\\alpha_0(t)^2 +\\cdots+ \\alpha_n(t)^2) dt,\n", "$$\n", "subject to the following constraint: one has $Z(1) = z$, where $Z : [0,1] \\to \\Omega$ is the path characterized by $Z(0) = z_0$ and for all $0 \\leq t \\leq 1$\n", "$$\n", " \\dot Z(t) = \\alpha_0(t) \\omega_0(Z(t)) + \\cdots + \\alpha_n(t) \\omega_n(Z(t)).\n", "$$\n", "\n", "**Eikonal equation.** The value function $U : \\Omega \\to R$ is, under suitable assumptions, the unique viscosity solution of the sub-Riemannian eikonal equation. More precisely, one has $U(z_0) = 0$, outflow boundary conditions on $\\partial \\Omega$, and for all $z \\in \\Omega$ \n", "$$\n", " \\|\\nabla u(z)\\|_{D(z)} = 1,\n", "$$\n", "where $D(z) = \\omega_0(z) \\omega_0(z)^T + \\cdots + \\omega_n(z) \\omega_n(z)^T$ is the sum of the self outer-products of the control vector fields.\n", "Note that $D(z)$ is a positive semi-definite matrix, which is rank deficient unless $(\\omega_0(z), \\cdots,\\omega_n(z))$ span the tangent space. The function $H(q,p) := \\frac 1 2 $ is referred to as the Hamiltonian of the system.\n", "\n", "**Riemannian relaxation.**\n", "Denote by $\\tilde \\omega_0, \\cdots , \\tilde \\omega_{\\tilde n}$ a family of vector fields which, together with $\\omega_0, \\cdots, \\omega_n$, span the tangent space to the state space. Let also $\\tilde D := \\tilde\\omega_0(z) \\tilde\\omega_0(z)^T + \\cdots + \\tilde\\omega_{\\tilde n}(z) \\cdots \\tilde \\omega_{\\tilde n}(z)^T$, and for any $\\epsilon >0$\n", "$$\n", " D_\\epsilon(z) := D(z) + \\epsilon^2 \\tilde D(z).\n", "$$\n", "Then $D_\\epsilon(z)$ is a positive definite (full rank) matrix, and the Riemannian eikonal equation \n", "$$\n", " \\|\\nabla u_\\epsilon(z)\\|_{D_\\epsilon(z)} = 1,\n", "$$\n", "is referred to as the $\\epsilon$-relaxation of the sub-Riemannian eikonal equation. The Riemannian eikonal equation can be discretized using the techniques which are standard in this series of notebooks, see below. For sufficiently small $\\epsilon>0$, the solution $u_\\epsilon$ to the Riemannian relaxation is close to the solution $u$ of the original sub-Riemannian equation, see the reference.\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 1.4 Numerical implementation\n", "\n", "**Discretization of the eikonal equation.**\n", "Assume that a tensor field $D$ admits a decomposition of the type\n", "$$\n", " D = \\sum_{i \\in I} \\lambda_i e_i e_i^T,\n", "$$\n", "where the index set $I$ is finite, $\\lambda_i \\geq 0$ is a non-negative weight, and $e_i \\in Z^d$ is an offset with integer coordinates, for each $i \\in I$.\n", "The pointwise dependency of $D=D(x)$, $\\lambda_i=\\lambda_i(x)$, $e_i=e_i(x)$ is omitted for readability.\n", "We discretize the Riemannian eikonal equation based on the following finite differences approximation:\n", "$$\n", " \\|\\nabla u(x)\\|_{D(x)}^2 \\approx h^{-2} \\sum_{i \\in I}\\lambda_i \\max\\{0,u(x)-u(x-h e_i), u(x)-u(x+h e_i)\\}^2.\n", "$$\n", "This resulting numerical scheme enjoys properties (monotony, causality) which allow to establish the convergence and stability of the method, and to implement it efficiently, see the reference above.\n", "\n", "**Tensor decomposition techniques and their limitations.**\n", "[Selling's](../Notebooks_Algo/TensorSelling.ipynb) and [Voronoi's](../Notebooks_Algo/TensorVoronoi.ipynb) matrix decompositions, are our main tools for obtaining the tensor decomposition presented above. They do require however, that the matrices $D(z)$, $z \\in \\Omega$, be positive definite. In fact, no such decomposition exists for semi-definite matrices in general.\n", "\n", "In the case of a sub-Riemannian eikonal equation, involving a rank deficient tensor field $D$, we thus apply the previous approach to the Riemannian relaxation $D_\\epsilon$.\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 2. One trailer\n", "\n", "We implement the rank one sub-Riemannian model corresponding to an omni-directional car with a single trailer." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 2.1 Control vector fields\n", "\n", "We reproduce the two control vector fields defining the model, whose mathematical expression is given in the first section." ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [], "source": [ "θs = xp.linspace(0,2*np.pi,96)" ] }, { "cell_type": "code", "execution_count": 9, "metadata": {}, "outputs": [], "source": [ "def ω(θ,κ=1):\n", " z,u = np.zeros_like(θ), np.ones_like(θ)\n", " return np.stack(ad.asarray([\n", " [ np.cos(θ),np.sin(θ), z],\n", " [-np.sin(θ),np.cos(θ),κ*u]\n", " ]),axis=1)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We also introduce one additional control vector field, used in the Riemannian relaxation, which together with the first two spans the tangent space." ] }, { "cell_type": "code", "execution_count": 10, "metadata": {}, "outputs": [], "source": [ "def ω_(θ,κ=1):\n", " u = np.ones_like(θ)\n", " return np.stack(ad.asarray([\n", " [κ*np.sin(θ),-κ*np.cos(θ),u]\n", " ]),axis=1)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The additional vector field is orthogonal to the two basic fields. This is a common choice, although it is not required.\n", "\n", "\n", "\n", "" ] }, { "cell_type": "code", "execution_count": 11, "metadata": {}, "outputs": [], "source": [ "assert allclose(lp.dot_VV(ω(θs),ω_(θs)),0)" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Shape of the dual metric (3, 3, 96)\n" ] } ], "source": [ "κ=2\n", "H = Hamiltonian(ω(θs,κ),ω_(θs,κ))\n", "print(f\"Shape of the dual metric {H.shape}\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Inspecting one of these matrices we find:\n", "* A ratio $0.1^2$ between the smallest and largest eigenvalue, which is expected (approximately) in view of the default relaxation parameter $\\epsilon = 0.1$ in `Hamiltonian`.\n", "* A tensor decomposition involving 6 non-zero weights with distinct offsets. This decomposition defines the stencil of the numerical scheme." ] }, { "cell_type": "code", "execution_count": 13, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Eigenvalues [ 0.05 1 5]\n" ] }, { "data": { "text/plain": [ "(array([0.168, 0.684, 0.358, 0.35, 0.0133, 0.149], dtype=float32),\n", " array([[ 1, -1, 0, 0, 1, -1],\n", " [-1, 0, -1, 1, 0, -1],\n", " [-3, 1, -1, 2, -2, 0]]))" ] }, "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hi = H[:,:,10]\n", "print(f\"Eigenvalues {np.linalg.eigvalsh(Hi)}\")\n", "Eikonal.VoronoiDecomposition(Hi)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 2.2 Input" ] }, { "cell_type": "code", "execution_count": 14, "metadata": {}, "outputs": [], "source": [ "hfmIn = Eikonal.dictIn({\n", " 'model':'Riemann3',\n", " 'seed':[0,0,0],\n", " 'periodic':[True,True,True],\n", "})\n", "hfmIn.SetRect([[-1,1],[-1,1],[-np.pi,np.pi]],dims = [101,101,96])\n", "hfmIn['origin'][2] -= hfmIn['gridScales'][2]/2 # Angular grid starts at -pi\n", "hfmIn.SetUniformTips([4,4,4])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Our metric is translation invariant w.r.t the first two coordinates $X = (X_0,X_1)$, only depending on the *last coordinate* the angle $\\theta$. This structure is relatively common, and the HFM library will take advantage of it to save some memory and compute less stencils.\n", "\n", "More generally, metrics depending only on some of the final coordinates $(x_k,...,x_n)$ are implicitly regarded invariant w.r.t the first coordinates, see also the two trailer case in the next section. (This is a form of broadcasting.)" ] }, { "cell_type": "code", "execution_count": 15, "metadata": {}, "outputs": [], "source": [ "_,_,aθ = hfmIn.Axes()\n", "assert np.allclose(aθ[0],-np.pi)" ] }, { "cell_type": "code", "execution_count": 16, "metadata": {}, "outputs": [], "source": [ "r=0.5 # Length of the trailer\n", "κ=1/r\n", "hfmIn['dualMetric'] = Metrics.Riemann(Hamiltonian(ω(aθ,κ), ω_(aθ,κ)))" ] }, { "cell_type": "code", "execution_count": 17, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 0.10970640182495117 seconds, and 108 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Displaying all the (planar projections of the) geodesics produces an elegant figure, but very little information." ] }, { "cell_type": "code", "execution_count": 18, "metadata": {}, "outputs": [], "source": [ "for geo in hfmOut['geodesics']:\n", " plt.plot(*geo[:2])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We show below the trajectory of the omni-directional vehicle in blue, the back of the trailer in red, and draw a family of lines representing the trailer body at different as time progresses." ] }, { "cell_type": "code", "execution_count": 19, "metadata": {}, "outputs": [], "source": [ "nx,ny=3,3\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " front = geo[:2]\n", " orient = geo[2]\n", " back = geo[:2] - r*ad.array((np.cos(orient),np.sin(orient)))\n", " plt.plot(*np.stack((front,back),axis=1)[:,:,::10]);\n", " plt.plot(*back,color='red') \n", " plt.plot(*front,color='blue')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 2.3 Special motions" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Other motions of interest include parallel parking, and half turn." ] }, { "cell_type": "code", "execution_count": 20, "metadata": {}, "outputs": [], "source": [ "hfmIn = Eikonal.dictIn({\n", " 'model':'Riemann3',\n", " 'seed':[0,0,0],\n", " 'periodic':[True,True,True],\n", " 'traits':{'niter_i':3}, # Performance related (iterations within a block)\n", "})\n", "#hfmIn.SetRect([[-1,1],[-1,1],[-np.pi,np.pi]],dims = [101,101,96])\n", "hfmIn.SetRect([[-1,1],[-1,1],[-np.pi,np.pi]],dims = [201,201,192])\n", "hfmIn['origin'][2] -= hfmIn['gridScales'][2]/2 # Angular grid starts at -pi\n", "\n", "_,_,aθ = hfmIn.Axes()\n", "r=0.5; κ=1/r # Length of the trailer, typical curvature\n", "hfmIn['dualMetric'] = Metrics.Riemann(Hamiltonian(ω(aθ,κ), ω_(aθ,κ),ϵ=0.05))" ] }, { "cell_type": "code", "execution_count": 21, "metadata": {}, "outputs": [], "source": [ "hfmIn['tips'] = [\n", " (0, 1.,0), # Parallel parking, far\n", " (0,0.5,0), # Parallel parking\n", " (0,0.2,0), # Parallel parking, close\n", " ( 0,0,np.pi), # Half turn, leaving head in place\n", " ( -r,0,np.pi), # Half turn, exchanging head and tail\n", " (-2*r,0,np.pi), # Half turn, leaving tail in place\n", "]" ] }, { "cell_type": "code", "execution_count": 22, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 0.44215846061706543 seconds, and 207 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "As before, we show the omni-directional head in blue, and the tail of the trailer in red.\n", "* First line : parallel parking.\n", "* Second line : half turn." ] }, { "cell_type": "code", "execution_count": 23, "metadata": {}, "outputs": [], "source": [ "nx,ny=3,2\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " front = geo[:2]\n", " orient = geo[2]\n", " back = geo[:2] - r*ad.array((np.cos(orient),np.sin(orient)))\n", " plt.plot(*np.stack((front,back),axis=1)[:,:,::10]);\n", " plt.plot(*back,color='red') \n", " plt.plot(*front,color='blue')\n", " plt.axis('equal')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 2.4 Comparison with the Reeds-Shepp model\n", "\n", "As mentioned in the first section, the Reeds-Shepp model and the single trailer model are isomorphic. The only difference being that the reference point is the back of the trailer in the Reeds-Shepp model. \n", "\n", "However, the numerical methods are distinct, since the Hamilonian of the Reeds-Shepp model benefits from a block diagonal structure. This is, in principle, an advantage, since it allows to reduce the number of offsets in the stencil (from 6 to 4), thus limiting numerical diffusion and the numerical cost." ] }, { "cell_type": "code", "execution_count": 24, "metadata": {}, "outputs": [], "source": [ "r=0.5; κ=1/r # Length of the trailer, typical curvature\n", "hfmIn = Eikonal.dictIn({\n", " 'model':'ReedsShepp2',\n", " 'seed':[0,0,0],\n", " 'xi':r,\n", "})\n", "hfmIn.SetRect([[-1,1],[-1,1]],dimx=201)\n", "hfmIn.nTheta = 192" ] }, { "cell_type": "code", "execution_count": 25, "metadata": {}, "outputs": [], "source": [ "hfmIn['tips'] = [\n", " (0, 1.,0), # Parallel parking, far\n", " (0,0.5,0), # Parallel parking\n", " (0,0.2,0), # Parallel parking, close\n", " (2*r,0,np.pi), # Half turn, leaving head in place\n", " ( r,0,np.pi), # Half turn, exchanging head and tail\n", " ( 0,0,np.pi), # Half turn, leaving tail in place\n", "]" ] }, { "cell_type": "code", "execution_count": 26, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 0.2818148136138916 seconds, and 81 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "As before, we show the omni-directional head in blue, and the tail of the trailer in red.\n", "* First line : parallel parking.\n", "* Second line : half turn." ] }, { "cell_type": "code", "execution_count": 27, "metadata": {}, "outputs": [], "source": [ "nx,ny=3,2\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " back = geo[:2] # Note : exchanging front and back\n", " orient = geo[2]\n", " front = geo[:2] + r*ad.array((np.cos(orient),np.sin(orient)))\n", " plt.plot(*np.stack((front,back),axis=1)[:,:,::10]);\n", " plt.plot(*back,color='red') \n", " plt.plot(*front,color='blue')\n", " plt.axis('equal')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 3. Two trailers\n", "\n", "The two trailer model is a rank two sub-Riemannian model, which for this reason is expected to be harder to solve numerically. " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 3.1 Control vector fields\n", "\n", "Again, we reproduce the two control vector fields defining the model, whose mathematical expression is given in the first section." ] }, { "cell_type": "code", "execution_count": 28, "metadata": {}, "outputs": [], "source": [ "angles = xp.linspace(0,2*np.pi,96)\n", "θs,ϕs = xp.meshgrid(angles,angles,indexing='ij')" ] }, { "cell_type": "code", "execution_count": 29, "metadata": {}, "outputs": [], "source": [ "def ω(θ,ϕ,κ0=1,κ1=1):\n", " z,u = np.zeros_like(θ), np.ones_like(θ)\n", " return np.stack(ad.asarray([\n", " [ np.cos(θ),np.sin(θ),z,κ1*np.sin(θ-ϕ)],\n", " [-np.sin(θ),np.cos(θ),κ0*u,z]\n", " ]),axis=1)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We also introduce two additional control vector fields, used in the Riemannian relaxation, which together with the first two span the tangent space." ] }, { "cell_type": "code", "execution_count": 30, "metadata": {}, "outputs": [], "source": [ "def ω_(θ,ϕ,κ0=1,κ1=1):\n", " z,u = np.zeros_like(θ), np.ones_like(θ)\n", " ρ = -κ1*np.sin(θ-ϕ)\n", " return np.stack(ad.asarray([\n", " [ρ*np.cos(θ), ρ*np.sin(θ),z,u],\n", " [κ0*np.sin(θ),-κ0*np.cos(θ),u,z]\n", " ]),axis=1)" ] }, { "cell_type": "code", "execution_count": 31, "metadata": {}, "outputs": [], "source": [ "r0=0.2; κ0=1/r0\n", "r1=0.2; κ1=1/r1\n", "κs = [κ0,κ1]" ] }, { "cell_type": "code", "execution_count": 32, "metadata": {}, "outputs": [], "source": [ "assert allclose(lp.dot_VV(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs)),0)" ] }, { "cell_type": "code", "execution_count": 33, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Shape of the dual metric (4, 4, 96, 96)\n" ] } ], "source": [ "H = Hamiltonian(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs))\n", "print(f\"Shape of the dual metric {H.shape}\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Looking at one of the Hamiltonian (symmetric positive definite) matrices, we find:\n", "* A ratio approximately $0.1^2$ between the smallest and largest eigenvalue, which is expected in view of the relaxation parameter $\\epsilon = 0.1$.\n", "* A decomposition involving $12$ non-zero weights, associated with distinct offsets, which define the structure of the finite difference scheme." ] }, { "cell_type": "code", "execution_count": 34, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Eigenvalues at an arbitrary point [0.0599 0.26 5.99 26]\n" ] }, { "data": { "text/plain": [ "(array([0.00612, 0.376, 0.00612, 0.321, 0.322, 0.0174, 0.171, 0.232, 0.00612, 0.0448, 5.24, 0.325], dtype=float32),\n", " array([[-1, -1, 0, 0, -1, 0, -1, -1, 0, -1, 0, 0],\n", " [-1, 0, -1, 1, 0, -1, -1, 1, -2, 1, 0, 1],\n", " [ 1, 3, -2, 3, 3, -2, 0, 5, -5, 6, 1, 3],\n", " [ 3, 1, 1, -2, 2, 2, 3, 0, 3, 0, 0, -1]]))" ] }, "execution_count": 34, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hi = H[:,:,12,19]\n", "print(f\"Eigenvalues at an arbitrary point {np.linalg.eigvalsh(Hi)}\")\n", "Eikonal.VoronoiDecomposition(Hi)" ] }, { "cell_type": "code", "execution_count": 35, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[ 1.15, -0.1, -3.53, -1.55],\n", " [ -0.1, 1.15, 3.47, -1.58],\n", " [-3.53, 3.47, 25, 0],\n", " [-1.55, -1.58, 0, 5]], dtype=float32)" ] }, "execution_count": 35, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hi" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 3.2 Input" ] }, { "cell_type": "code", "execution_count": 36, "metadata": {}, "outputs": [], "source": [ "hfmIn = Eikonal.dictIn({\n", " 'model':'Riemann4',\n", " 'seed':[0,0,0,0],\n", " 'periodic':[True,True,True,True],\n", " 'traits':{'niter_i':2,'shape_i':(4,4,4,2)},\n", " 'raiseOnNonConvergence':False,\n", " 'exportValues':True,\n", " 'exportGeodesicFlow':True\n", "})\n", "hfmIn.SetRect([[-0.5,0.5],[-0.5,0.5],[-np.pi,np.pi],[-np.pi,np.pi]],dims=[81,81,96,96]) \n", "hfmIn['origin'][2:] -= hfmIn['gridScales'][2:]/2 # Angular grid starts at -pi\n", "hfmIn.SetUniformTips([4,4,4,1])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Our metric is translation invariant w.r.t the first two coordinates $X = (X_0,X_1)$, only depending on the *last two coordinates* the angle $\\theta$ and $\\phi$. This structure is relatively common, and the HFM library will take advantage of it to save some memory and compute less stencils." ] }, { "cell_type": "code", "execution_count": 37, "metadata": {}, "outputs": [], "source": [ "_,_,aθ,aϕ = hfmIn.Axes()\n", "assert np.allclose(aθ[0],-np.pi) and np.allclose(aϕ[0],-np.pi)\n", "θs,ϕs = xp.meshgrid(aθ,aϕ,indexing='ij')" ] }, { "cell_type": "code", "execution_count": 38, "metadata": {}, "outputs": [], "source": [ "r0=0.2; κ0=1/r0\n", "r1=0.2; κ1=1/r1\n", "κs = [κ0,κ1]\n", "hfmIn['dualMetric'] = Metrics.Riemann(Hamiltonian(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs),ϵ=0.04 ))" ] }, { "cell_type": "code", "execution_count": 39, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 5.954641819000244 seconds, and 73 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The spatial projections of the geodesics are even less informative than before." ] }, { "cell_type": "code", "execution_count": 49, "metadata": {}, "outputs": [], "source": [ "for geo in hfmOut['geodesics']:\n", " plt.plot(*geo[:2])" ] }, { "cell_type": "code", "execution_count": 50, "metadata": {}, "outputs": [], "source": [ "nx,ny=2,2\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " front = geo[:2]\n", " orient = geo[2]; back0 = front - r0*ad.array((np.cos(orient),np.sin(orient)))\n", " orient = geo[3]; back1 = back0 - r1*ad.array((np.cos(orient),np.sin(orient)))\n", " \n", " plt.plot(*np.stack((front,back0),axis=1)[:,:,::10],color='gray');\n", " plt.plot(*np.stack((back0,back1),axis=1)[:,:,::10],color='lightgray');\n", "\n", " plt.plot(*front,color='blue')\n", " plt.plot(*back0,color='red') \n", " plt.plot(*back1,color='purple') \n", " plt.axis('equal')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 3.3 Special motions" ] }, { "cell_type": "code", "execution_count": 51, "metadata": {}, "outputs": [], "source": [ "hfmIn['tips'] = [\n", " (0., 0.5,0.,0.), # Parallel parking, far\n", " (0.,0.35,0.,0.), # Parallel parking\n", " (0., 0.2,0.,0.), # Parallel parking, close\n", " (0,0,np.pi,np.pi), # Half turn, leaving head in place\n", " ( -r0,0,np.pi,np.pi), # Half turn, exchanging head and middle\n", " (-r0-r1,0,np.pi,np.pi), # Half turn, exchanging the head and tail\n", "]\n", "hfmIn['periodic']=[False,True,True,True]" ] }, { "cell_type": "code", "execution_count": 52, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 7.45104455947876 seconds, and 83 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The (approximate) optimal motions for parallel parking a half turn are shown below. Note that they feature many cusps.\n", "\n", "**Failed close parallel parking.** In this test case, it is more advantageous to use the complementary vector fields, despite their strong penalization, than the control vector fields. That is why we see such an incorrect parallel translation." ] }, { "cell_type": "code", "execution_count": 53, "metadata": {}, "outputs": [], "source": [ "nx,ny=3,2\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " front = geo[:2]\n", " orient = geo[2]; back0 = front - r0*ad.array((np.cos(orient),np.sin(orient)))\n", " orient = geo[3]; back1 = back0 - r1*ad.array((np.cos(orient),np.sin(orient)))\n", " \n", " plt.plot(*np.stack((front,back0),axis=1)[:,:,::10],color='gray');\n", " plt.plot(*np.stack((back0,back1),axis=1)[:,:,::10],color='lightgray');\n", "\n", " plt.plot(*front,color='blue')\n", " plt.plot(*back0,color='red') \n", " plt.plot(*back1,color='purple')\n", " plt.axis('equal')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 3.4 Comparison with the Reeds-Shepp car\n", "\n", "An omni-directional vehicle with two trailers amounts to a Reeds-Shepp car with a single trailer. Similarly to the previous case, the isomorphism is obtained by using the back of the first trailer as a reference point.\n", "\n", "We use this approach to obtain an alternative discretization of the problem of interest, and validate our numerical experiments." ] }, { "cell_type": "code", "execution_count": 54, "metadata": {}, "outputs": [], "source": [ "angles = xp.linspace(0,2*np.pi,96)\n", "θs,ϕs = xp.meshgrid(angles,angles,indexing='ij')" ] }, { "cell_type": "code", "execution_count": 55, "metadata": {}, "outputs": [], "source": [ "def ω(θ,ϕ,κ0=1,κ1=1):\n", " z,u = np.zeros_like(θ), np.ones_like(θ)\n", " return np.stack(ad.asarray([\n", " [np.cos(θ),np.sin(θ),z,κ1*np.sin(θ-ϕ)],\n", " [z,z,κ0*u,z]\n", " ]),axis=1)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We also introduce two additional control vector fields, used in the Riemannian relaxation, which together with the first two span the tangent space." ] }, { "cell_type": "code", "execution_count": 56, "metadata": {}, "outputs": [], "source": [ "def ω_(θ,ϕ,κ0=1,κ1=1):\n", " z,u = np.zeros_like(θ), np.ones_like(θ)\n", " ρ = -κ1*np.sin(θ-ϕ)\n", " return np.stack(ad.asarray([\n", " [ρ*np.cos(θ), ρ*np.sin(θ),z,u],\n", " [ -np.sin(θ), np.cos(θ),z,z]\n", " ]),axis=1)" ] }, { "cell_type": "code", "execution_count": 57, "metadata": {}, "outputs": [], "source": [ "r0=0.2; κ0=1/r0\n", "r1=0.2; κ1=1/r1\n", "κs = [κ0,κ1]" ] }, { "cell_type": "code", "execution_count": 58, "metadata": {}, "outputs": [], "source": [ "assert allclose(lp.dot_VV(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs)),0)" ] }, { "cell_type": "code", "execution_count": 59, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Shape of the dual metric (4, 4, 96, 96)\n" ] } ], "source": [ "H = Hamiltonian(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs))\n", "print(f\"Shape of the dual metric {H.shape}\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Looking at one of the Hamiltonian (symmetric positive definite) matrices, we find:\n", "* A ratio approximately $0.1^2$ between the smallest and largest eigenvalue, which is expected in view of the relaxation parameter $\\epsilon = 0.1$.\n", "* A decomposition involving $7$ non-zero weights, associated with distinct offsets, which define the structure of the finite difference scheme. There are also $5$ vanishing weights (up to numerical precision) in the decomposition." ] }, { "cell_type": "code", "execution_count": 60, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Eigenvalues at an arbitrary point [ 0.01 0.0599 5.99 25]\n" ] }, { "data": { "text/plain": [ "(array([0.00148, 0.01, 0, 0.00865, 0.509, 25, 0.0108, 0.326, 0, 2.98e-07, 0, 0], dtype=float32),\n", " array([[ 1, 0, 1, 0, 1, 0, 1, 0, -1, 1, 0, 0],\n", " [ 0, -1, 0, -1, 1, 0, 1, 0, -1, 1, 0, 0],\n", " [ 0, 0, 1, 0, 0, -1, 0, 0, -1, 1, 0, 0],\n", " [-1, 1, -1, 2, -3, 0, -2, -1, 2, -3, 0, 0]]))" ] }, "execution_count": 60, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hi = H[:,:,12,19]\n", "print(f\"Eigenvalues at an arbitrary point {np.linalg.eigvalsh(Hi)}\")\n", "Eikonal.VoronoiDecomposition(Hi)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Upon inspection, and a permutation of the last two axes, we find that the Hamiltonian matrix has a block diagonal structure, with blocks of sizes $3\\times 3$ and $1 \\times 1$. Recall that Voronoi's decomposition of a $3 \\times 3$ matrix uses $6$ weights and offsets, and that one additional weight and offset is needed for the last $1 \\times 1$ block. This explains why only 7 non-zero weights are involved in the decomposition of the the hamiltonians of this model." ] }, { "cell_type": "code", "execution_count": 61, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[0.521, 0.52, -1.55, 0],\n", " [ 0.52, 0.539, -1.58, 0],\n", " [-1.55, -1.58, 5, 0],\n", " [ 0, 0, 0, 25]], dtype=float32)" ] }, "execution_count": 61, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hi[[0,1,3,2]][:,[0,1,3,2]]" ] }, { "cell_type": "code", "execution_count": 62, "metadata": {}, "outputs": [], "source": [ "hfmIn = Eikonal.dictIn({\n", " 'model':'Riemann4',\n", " 'seed':[0,0,0,0],\n", " 'traits':{'niter_i':2,'shape_i':(4,4,4,2)},\n", " 'raiseOnNonConvergence':False,\n", " 'exportValues':True,\n", "})\n", "hfmIn.SetRect([[-0.5,0.5],[-0.5,0.5],[-np.pi,np.pi],[-np.pi,np.pi]],dims=[81,81,96,96]) \n", "\n", "hfmIn['origin'][2:] -= hfmIn['gridScales'][2:]/2 # Angular grid starts at -pi\n", "_,_,aθ,aϕ = hfmIn.Axes()\n", "θs,ϕs = xp.meshgrid(aθ,aϕ,indexing='ij')" ] }, { "cell_type": "code", "execution_count": 63, "metadata": {}, "outputs": [], "source": [ "r0=0.2; κ0=1/r0\n", "r1=0.2; κ1=1/r1\n", "κs = [κ0,κ1]\n", "hfmIn['dualMetric'] = Metrics.Riemann(Hamiltonian(ω(θs,ϕs,*κs),ω_(θs,ϕs,*κs),ϵ=0.06 ))" ] }, { "cell_type": "code", "execution_count": 64, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[ 0.9, 0.304, 0, 0.92],\n", " [0.304, 0.107, 0, 0.312],\n", " [ 0, 0, 25, 0],\n", " [ 0.92, 0.312, 0, 0.955]], dtype=float32)" ] }, "execution_count": 64, "metadata": {}, "output_type": "execute_result" } ], "source": [ "hfmIn['dualMetric'].m[:,:,5,8]" ] }, { "cell_type": "code", "execution_count": 65, "metadata": {}, "outputs": [], "source": [ "hfmIn['tips'] = [\n", " (0., 0.5,0.,0.), # Parallel parking, far\n", " (0.,0.35,0.,0.), # Parallel parking\n", " (0., 0.2,0.,0.), # Parallel parking, close\n", " (2*r0,0,np.pi,np.pi), # Half turn, leaving head in place\n", " ( r0,0,np.pi,np.pi), # Half turn, exchanging head and middle\n", " (r0-r1,0,np.pi,np.pi), # Half turn, exchanging the head and tail\n", "]\n", "hfmIn['periodic']=[False,True,True,True]" ] }, { "cell_type": "code", "execution_count": 66, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "Setting the kernel traits.\n", "Preparing the domain data (shape,metric,...)\n", "Preparing the problem rhs (cost, seeds,...)\n", "Preparing the GPU kernel\n", "Running the eikonal GPU kernel\n", "GPU kernel eikonal ran for 7.501668214797974 seconds, and 84 iterations.\n", "Post-Processing\n" ] } ], "source": [ "hfmOut = hfmIn.Run()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We recognize the motions, although accuracy is not always at its best, with the following differences:\n", "* The close parllel parking is slightly better extracted than before... Although one clearly sees that the motion of the second trailer substantially violates the sub-Riemannian constraint (side motion, not aligned with the trailer).\n", "* The motion of the front of the car is less smooth, probably due to discretization." ] }, { "cell_type": "code", "execution_count": 67, "metadata": {}, "outputs": [], "source": [ "nx,ny=3,2\n", "plt.figure(figsize=(16,12*ny/nx))\n", "for i,geo in enumerate(hfmOut['geodesics']):\n", " if i>=nx*ny: break\n", " plt.subplot(ny,nx,1+i)\n", " \n", " back0 = geo[:2] \n", " orient = geo[2]; front = back0 + r0*ad.array((np.cos(orient),np.sin(orient)))\n", " orient = geo[3]; back1 = back0 - r1*ad.array((np.cos(orient),np.sin(orient)))\n", " \n", " plt.plot(*np.stack((front,back0),axis=1)[:,:,::10],color='gray');\n", " plt.plot(*np.stack((back0,back1),axis=1)[:,:,::10],color='lightgray');\n", "\n", " plt.plot(*front,color='blue')\n", " plt.plot(*back0,color='red') \n", " plt.plot(*back1,color='purple') \n", " plt.axis('equal')" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "celltoolbar": "Format de la Cellule Texte Brut", "kernelspec": { "display_name": "Python 3 (ipykernel)", "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.10.8" } }, "nbformat": 4, "nbformat_minor": 4 }