iLQR_distMaintenance.m added

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

python/iLQR_obstacle.py

 '''
     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()
+plt.show()
\ No newline at end of file