agd.ExportedCode.Notebooks_Algo.RollingBall_Models

18def norm(arr,ord=2,axis=None,keepdims=False,averaged=False):
19	"""
20	Returns L^p norm of array, seen as a vector, w.r.t. weights.
21	Defined as : (sum_i x[i]^p)^(1/p)
22
23	Remark : not a matrix operator norm
24	
25	Inputs:
26	 - ord : exponent p
27	 - axis : int or None, axis along which to compute the norm. 
28	 - keepdims : wether to keep singleton dimensions.
29	 - averaged : wether to introduce a normalization factor, so that norm(ones(...))=1
30
31	Compatible with automatic differentiation.
32	"""
33	arr = ad_generic.array(arr)
34	if ord==np.inf or ord%2!=0:
35		try: arr = np.abs(arr)
36		except TypeError: arr = np.vectorize(np.abs)(arr)
37	if ord==np.inf: return np.max(arr,axis=axis,keepdims=keepdims)
38	sum_pow = np.sum(arr**ord,axis=axis,keepdims=keepdims)
39	
40	if averaged:
41		size = arr.size if axis is None else arr.shape[axis]
42		sum_pow/=size
43
44	return sum_pow**(1./ord)

36def rotation3_from_sphere3(q):
37    """Produces the rotation associated with a unit quaternion."""
38    qr,qi,qj,qk = q
39    return 2*ad.array([
40        [0.5-(qj**2+qk**2), qi*qj-qk*qr, qi*qk+qj*qr],
41        [qi*qj+qk*qr, 0.5-(qi**2+qk**2), qj*qk-qi*qr],
42        [qi*qk-qj*qr, qj*qk+qi*qr, 0.5-(qi**2+qj**2)]])

44def sphere3_from_rotation3(r):
45    """Produces the unit quaternion, with positive real part, associated with a rotation."""
46    qr = np.sqrt(lp.trace(r)+1.)/2.
47    qi = (r[2,1]-r[1,2])/(4*qr)
48    qj = (r[0,2]-r[2,0])/(4*qr)
49    qk = (r[1,0]-r[0,1])/(4*qr)
50    return ad.array([qr,qi,qj,qk])

19def sphere_from_plane(e):
20    """Produces a point in the unit sphere by projecting a point in the equator plane."""
21    e = ad.asarray(e)
22    e2 = lp.dot_VV(e,e)
23    return ad.array([1.-e2,*(2*e)])/(1.+e2)

24def plane_from_sphere(q):
25    """Produces a point in the equator plane from a point in the unit sphere."""
26    e2 = (1-q[0])/(1+q[0])
27    return q[1:]*((1+e2)/2.)

52def ball3_from_rotation3(r,qRef=None):
53    """Produces an euclidean point from a rotation, 
54    selecting in the intermediate step the quaternion 
55    in the same hemisphere as qRef. (Defaults to southern.)"""
56    q = sphere3_from_rotation3(r)
57    if qRef is not None: q[:,lp.dot_VV(q,qRef)<0] *= -1
58    return plane_from_sphere(q)

60def rotation3_from_ball3(e): 
61    """Produces a rotation from an euclidean point. 
62    Also returns the intermediate quaternion."""
63    q = sphere_from_plane(e)
64    return rotation3_from_sphere3(q),q

20def antisym(a,b,c):
21    z=np.zeros_like(a)
22    return ad.array([[z, -c, b], [c, z, -a], [-b, a, z]])

24def exp_antisym(a,b,c):
25    """Matrix exponential of antisym(a,b,c).
26    Note : (a,b,c) is the axis of rotation."""
27    s = ad.asarray(a**2+b**2+c**2)
28    s[s==0]=1e-20 # Same trick as in numpy's sinc function ...
29    sq = np.sqrt(s)
30    co,si = np.cos(sq),np.sin(sq)
31    cosc,sinc = (1-co)/s,si/sq    
32    return ad.array([
33        [co+cosc*a**2, cosc*a*b-sinc*c, cosc*a*c+sinc*b],
34        [cosc*a*b+sinc*c, co+cosc*b**2, cosc*b*c-sinc*a],
35        [cosc*a*c-sinc*b, cosc*b*c+sinc*a, co+cosc*c**2]])

37def advance(state,control):
38    """Move from a state to another by applying a control during a unit time"""
39    state,control = map(ad.asarray,(state,control))
40    state_physical = state[:-3]
41    state_physical = state_physical + 0.25*control[:len(state_physical)] # Additive action on the physical state
42    
43    state_angular,qRef = rotation_from_euclidean(state[-3:])
44    state_angular = lp.dot_AA(state_angular,exp_antisym(*control)) # Left invariant action
45    
46    return np.concatenate([state_physical,euclidean_from_rotation(state_angular,qRef)],axis=0)

48def make_hamiltonian(controls,advance=advance):
49    """Produces the hamiltonian function associated to a sub-Riemannian model, 
50    defined by its controls and the advance function"""
51    def hamiltonian(state):
52        """The hamiltonian, a quadratic form on the co-tangent space"""
53        # Array formatting to apply to several states simultanously
54        state=ad.asarray(state); controls_ = fd.as_field(controls,state.shape[1:],depth=1) 
55        
56        grad = advance(state,controls_).gradient()
57        return lp.dot_AA(lp.transpose(grad),grad)
58    return hamiltonian