agd.ExportedCode.Notebooks_FMM.ElasticaVariants 

 1# Code automatically exported from notebook ElasticaVariants.ipynb in directory Notebooks_FMM
 2# Do not modify
 3from ... import Eikonal 
 4from ... import AutomaticDifferentiation as ad
 5from ... import Plotting
 6from ... import FiniteDifferences as fd
 7from agd.ExportedCode.Notebooks_FMM.DubinsState import control_source
 8
 9import numpy as np
10from matplotlib import pyplot as plt
11π=np.pi
12
13def elastica_control(ϕ): 
14    """
15    Control for the elastica model (two dimensional projection).
16    Boundary point of the disk of radius 1/2 and center (1/2,0).
17    """
18    return np.cos(ϕ)*np.array((np.cos(ϕ),np.sin(ϕ)))
19
20def elastica_controls(I,ϕmin=-π/2,ϕmax=π/2,pad=False):
21    """
22    Controls for the elastica model, regularly sampled.
23    - I : number of controls
24    - ϕmin, ϕmax : smallest and largest control
25    - pad : add a zero control.
26    """
27    if ϕmin==-π/2: ϕmin = -π/2+π/I
28    if ϕmax== π/2: ϕmax =  π/2-π/I
29    ϕ=np.linspace(ϕmin,ϕmax,I)
30    if pad: ϕ = np.pad(ϕ,(1,1),constant_values=π/2)
31    return elastica_control(ϕ)
32
33def with_prior(c,ξ=1,κ=0):
34    """
35    Adjusts the control vectors to account for the following priors : 
36    - ξ the typical turning radius, 
37    - κ the reference curvature.
38    """
39    ds,dθ = c
40    return np.array((ds,(dθ/ξ+κ*ds)))
41
42def embed(c,θ):
43    """
44    Embed a two dimensional control (α,β) in R^2xR as (α n(θ),β)
45    where n(θ) = (cos θ, sin θ)
46    """
47    ds,dθ = c
48    return np.array((ds*np.cos(θ),ds*np.sin(θ),dθ*np.ones_like(θ)))
49
50fejer_weights_ = [
51    [],[2.],[1.,1.],[0.444444, 1.11111, 0.444444],[0.264298, 0.735702, 0.735702, 0.264298],
52    [0.167781, 0.525552, 0.613333, 0.525552, 0.167781],[.118661, 0.377778, 0.503561, 0.503561, 0.377778, 0.118661],
53    [.0867162, 0.287831, 0.398242, 0.454422, 0.398242, 0.287831, 0.0867162],
54    [.0669829, 0.222988, 0.324153, 0.385877, 0.385877, 0.324153, 0.222988, 0.0669829],
55    [.0527366, 0.179189, 0.264037, 0.330845, 0.346384, 0.330845, 0.264037, 0.179189, 0.0527366]
56]
57def fejer_weights(I):
58    """
59    Returns the Fejer weights, for computing a cosine weighted integral.
60    If I>=10, returns an approximation based on the midpoint rule.
61    """
62    if I<len(fejer_weights_): return fejer_weights_[I]
63    x=np.linspace(0,π,I,endpoint=False)
64    return np.cos(x)-np.cos(x+π/I)
65
66def elastica_terms(I,ϕmin=-π/2,ϕmax=π/2):
67    midpoints = Eikonal.CenteredLinspace(ϕmin,ϕmax,I)
68    weights = np.array(fejer_weights(I))
69    return np.array((np.cos(midpoints),np.sin(midpoints)))*np.sqrt((3/4)*weights)
70
71def in_place_rotation_controls(): return np.array(((0,0,1),(0,0,-1)))
72state_transition_base_cost = np.array(((0,1),(1,0))) # discrete cost of jumping from one state to another
73
74def embed4(c,θ,κ):
75    """
76    Embed a two dimensional control (α,β) in R^2 x R x R as (α n(θ),α κ, β)
77    where n(θ) = (cos θ, sin θ)
78    """
79    ds,dθ = c
80    uθ,uκ = np.ones_like(θ),np.ones_like(κ)
81    return np.array((ds*np.cos(θ)*uκ,ds*np.sin(θ)*uκ,ds*uθ*κ,dθ*uθ*uκ))
π = 3.141592653589793
def elastica_control(φ): 
14def elastica_control(ϕ): 
15    """
16    Control for the elastica model (two dimensional projection).
17    Boundary point of the disk of radius 1/2 and center (1/2,0).
18    """
19    return np.cos(ϕ)*np.array((np.cos(ϕ),np.sin(ϕ)))

Control for the elastica model (two dimensional projection). Boundary point of the disk of radius 1/2 and center (1/2,0).

def elastica_controls(I, φmin=-1.5707963267948966, φmax=1.5707963267948966, pad=False): 
21def elastica_controls(I,ϕmin=-π/2,ϕmax=π/2,pad=False):
22    """
23    Controls for the elastica model, regularly sampled.
24    - I : number of controls
25    - ϕmin, ϕmax : smallest and largest control
26    - pad : add a zero control.
27    """
28    if ϕmin==-π/2: ϕmin = -π/2+π/I
29    if ϕmax== π/2: ϕmax =  π/2-π/I
30    ϕ=np.linspace(ϕmin,ϕmax,I)
31    if pad: ϕ = np.pad(ϕ,(1,1),constant_values=π/2)
32    return elastica_control(ϕ)

Controls for the elastica model, regularly sampled.

  • I : number of controls
  • ϕmin, ϕmax : smallest and largest control
  • pad : add a zero control.
def with_prior(c, ξ=1, κ=0): 
34def with_prior(c,ξ=1,κ=0):
35    """
36    Adjusts the control vectors to account for the following priors : 
37    - ξ the typical turning radius, 
38    - κ the reference curvature.
39    """
40    ds,dθ = c
41    return np.array((ds,(dθ/ξ+κ*ds)))

Adjusts the control vectors to account for the following priors :

  • ξ the typical turning radius,
  • κ the reference curvature.
def embed(c, θ): 
43def embed(c,θ):
44    """
45    Embed a two dimensional control (α,β) in R^2xR as (α n(θ),β)
46    where n(θ) = (cos θ, sin θ)
47    """
48    ds,dθ = c
49    return np.array((ds*np.cos(θ),ds*np.sin(θ),dθ*np.ones_like(θ)))

Embed a two dimensional control (α,β) in R^2xR as (α n(θ),β) where n(θ) = (cos θ, sin θ)

fejer_weights_ = [[], [2.0], [1.0, 1.0], [0.444444, 1.11111, 0.444444], [0.264298, 0.735702, 0.735702, 0.264298], [0.167781, 0.525552, 0.613333, 0.525552, 0.167781], [0.118661, 0.377778, 0.503561, 0.503561, 0.377778, 0.118661], [0.0867162, 0.287831, 0.398242, 0.454422, 0.398242, 0.287831, 0.0867162], [0.0669829, 0.222988, 0.324153, 0.385877, 0.385877, 0.324153, 0.222988, 0.0669829], [0.0527366, 0.179189, 0.264037, 0.330845, 0.346384, 0.330845, 0.264037, 0.179189, 0.0527366]]
def fejer_weights(I): 
58def fejer_weights(I):
59    """
60    Returns the Fejer weights, for computing a cosine weighted integral.
61    If I>=10, returns an approximation based on the midpoint rule.
62    """
63    if I<len(fejer_weights_): return fejer_weights_[I]
64    x=np.linspace(0,π,I,endpoint=False)
65    return np.cos(x)-np.cos(x+π/I)

Returns the Fejer weights, for computing a cosine weighted integral. If I>=10, returns an approximation based on the midpoint rule.

def elastica_terms(I, φmin=-1.5707963267948966, φmax=1.5707963267948966): 
67def elastica_terms(I,ϕmin=-π/2,ϕmax=π/2):
68    midpoints = Eikonal.CenteredLinspace(ϕmin,ϕmax,I)
69    weights = np.array(fejer_weights(I))
70    return np.array((np.cos(midpoints),np.sin(midpoints)))*np.sqrt((3/4)*weights)
def in_place_rotation_controls(): 
72def in_place_rotation_controls(): return np.array(((0,0,1),(0,0,-1)))
state_transition_base_cost = array([[0, 1], [1, 0]])
def embed4(c, θ, κ): 
75def embed4(c,θ,κ):
76    """
77    Embed a two dimensional control (α,β) in R^2 x R x R as (α n(θ),α κ, β)
78    where n(θ) = (cos θ, sin θ)
79    """
80    ds,dθ = c
81    uθ,uκ = np.ones_like(θ),np.ones_like(κ)
82    return np.array((ds*np.cos(θ)*uκ,ds*np.sin(θ)*uκ,ds*uθ*κ,dθ*uθ*uκ))

Embed a two dimensional control (α,β) in R^2 x R x R as (α n(θ),α κ, β) where n(θ) = (cos θ, sin θ)