diff --git a/matlab/iLQR_obstacle.m b/matlab/iLQR_obstacle.m index b36c748baa23a146f9ab9e6878f1e75367114015..7e09ee8e5d66f40d5103b1b02638698df68aa8d9 100644 --- a/matlab/iLQR_obstacle.m +++ b/matlab/iLQR_obstacle.m @@ -23,7 +23,7 @@ function iLQR_obstacle %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% param.dt = 1E-2; %Time step size -param.nbData = 101; %Number of datapoints +param.nbData = 100; %Number of datapoints param.nbIter = 300; %Maximum number of iterations for iLQR param.nbPoints = 1; %Number of viapoints param.nbObstacles = 2; %Number of obstacles diff --git a/python/iLQR_obstacle.py b/python/iLQR_obstacle.py index 9396939758c7d545a31818f87b276f515b562abf..6de5c063bee4a32e215dc95e64bba4a1bf7906ed 100644 --- a/python/iLQR_obstacle.py +++ b/python/iLQR_obstacle.py @@ -1,7 +1,7 @@ ''' Batch iLQR with obstacle avoidance - Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/ + Copyright (c) 2021 Idiap Research Institute, https://www.idiap.ch/ Written by Jeremy Maceiras <jeremy.maceiras@idiap.ch>, Sylvain Calinon <https://calinon.ch> @@ -45,11 +45,11 @@ def f_avoid(x,param): if ftmp > 0: f.append(ftmp) - Jtmp = -1*(param.U_obs[j] @ e).T.reshape((-1,1)) + Jtmp = -(param.U_obs[j] @ e).T.reshape((-1,1)) J2 = np.zeros(( J.shape[0] + Jtmp.shape[0] , J.shape[1] + Jtmp.shape[1] )) J2[:J.shape[0],:J.shape[1]] = J J2[-Jtmp.shape[0]:,-Jtmp.shape[1]:] = Jtmp - J = J2 # Numpy does not provid a blockdiag function... + J = J2 # Numpy does not provide a 'blockdiag' function... idx.append( i*param.nbVarU + np.array(range(param.nbVarU)) ) idt.append(i) @@ -58,13 +58,13 @@ def f_avoid(x,param): idt = np.array(idt) return f,J.T,idx,idt -# General param parameters +# General parameters # =============================== param = lambda: None # Lazy way to define an empty class in python param.dt = 1e-2 # Time step length -param.nbData = 101 # Number of datapoints -param.nbIter = 20 # Maximum number of iterations for iLQR +param.nbData = 100 # Number of datapoints +param.nbIter = 300 # Maximum number of iterations for iLQR param.nbObstacles = 2 # Number of obstacles param.nbPoints = 1 # Number of viapoints param.nbVarX = 2 # State space dimension (x1,x2,x3) @@ -109,7 +109,7 @@ R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r # System parameters # =============================== -# Time occurence of viapoints +# Time occurrence of viapoints tl = np.linspace(0,param.nbData,param.nbPoints+1) tl = np.rint(tl[1:]).astype(np.int64)-1 idx = np.array([ i + np.arange(0,param.nbVarX,1) for i in (tl* param.nbVarX)]) @@ -163,7 +163,7 @@ for i in range( param.nbIter ): if np.linalg.norm(alpha * du) < 1e-2: # Early stop condition break -# Ploting +# Plotting # =============================== plt.figure() @@ -172,27 +172,27 @@ plt.gca().set_aspect('equal', adjustable='box') # Get trajectory x = Su0 @ u + Sx0 @ x0 -x = x.reshape( ( param.nbData , param.nbVarX) ) +x = x.reshape( (param.nbData, param.nbVarX) ) -plt.scatter(x[0,0],x[0,1],c='black',s=100) +plt.scatter(x[0,0], x[0,1], c='black', s=40) # Plot targets for i in range(param.nbPoints): xt = param.Mu[i] - plt.scatter(xt[0],xt[1],c='blue',s=100) + plt.scatter(xt[0], xt[1], c='red', s=40) -# Plot obstactles -al = np.linspace(-np.pi,np.pi,50) +# Plot obstacles +al = np.linspace(-np.pi, np.pi, 50) ax = plt.gca() for i in range(param.nbObstacles): D,V = np.linalg.eig(param.S_obs[i]) D = np.diag(D) R = np.real(V@np.sqrt(D+0j)) msh = (R @ np.array([np.cos(al),np.sin(al)])).T + param.Obs[i][:2] - p=patches.Polygon(msh,closed=True) + p = patches.Polygon(msh, closed=True) ax.add_patch(p) -plt.plot(x[:,0],x[:,1],c='black') -plt.scatter(x[::10,0],x[::10,1],c='black') +plt.plot(x[:,0], x[:,1], c='black') +plt.scatter(x[::10,0], x[::10,1], c='black', s=10) -plt.show() \ No newline at end of file +plt.show()