diff --git a/python/FD.py b/python/FD.py
index 0adadbd79292174121300efdbe23fbd617e7684d..0fde6ffac1b303428fc2fc6d61136d3418c96c9a 100644
--- a/python/FD.py
+++ b/python/FD.py
@@ -29,23 +29,18 @@ import matplotlib.animation as animation
 # Helper functions
 # ===============================
 
-# NB: Because of the use of animation, we need to set param as a global variable
-
-# Forward Kinematics for all joints
-def fkin0(x):
-    global param
-    T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
-    T2 = np.tril(np.matlib.repmat(param.l,len(x),1))
-    f = np.vstack(( 
-        T2 @ np.cos(T@x),
-        T2 @ np.sin(T@x)
-    )).T
-    f = np.vstack(( 
-        np.zeros(2),
-        f
-    ))
-    return f
-
+# Forward kinematics for all joints (in robot coordinate system)
+def fkin0(x, param): 
+	L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+	f = np.vstack([
+		L @ np.diag(param.l) @ np.cos(L @ x),
+		L @ np.diag(param.l) @ np.sin(L @ x)
+	])
+	f = np.hstack([np.zeros([2,1]), f])
+	return f
+
+
+# NB: Because of the use of matplotlib.animation, we need to set param as a global variable
 # Initialization of the plot
 def init():
     global param
@@ -53,12 +48,13 @@ def init():
     ax.set_ylim(-np.sum(param.l) - 0.1, np.sum(param.l) + 0.1)
     return ln1, ln2
 
+# NB: Because of the use of matplotlib.animation, we need to set param as a global variable
 # Updating the values in the plot
 def animate(i):
     global param
-    f = fkin0(x[0:param.nbVarX,i])
-    ln1.set_data(f[:,0], f[:,1])
-    ln2.set_data(f[:,0], f[:,1])
+    f = fkin0(x[0:param.nbVarX,i], param)
+    ln1.set_data(f[0,:], f[1,:])
+    ln2.set_data(f[0,:], f[1,:])
     return ln1, ln2
 
 
@@ -67,7 +63,6 @@ def animate(i):
 param = lambda: None # Lazy way to define an empty class in python
 param.dt = 1e-2 # Time step length
 param.nbData = 500 # Number of datapoints
-param.nbPoints = 2 # Number of viapoints
 param.nbVarX = 3 # State space dimension (x1,x2,x3)
 param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3)
 param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation)
@@ -91,7 +86,6 @@ u = np.zeros([param.nbVarU*(param.nbData - 1), 1]) #Input commands for the whole
 x = x0
 # Forward Dynamics
 for t in range(param.nbData - 1):
-        
     # Computation in matrix form of G,M, and C
     G =-np.reshape(np.sum(Tm, 1), [param.nbVarX, 1]) * l.T * np.cos(T @ np.reshape(x[0:param.nbVarX, t], [param.nbVarX, 1])) * param.gravity
     G = T.T @ G
@@ -115,6 +109,6 @@ plt.axis("off")
 plt.gca().set_aspect('equal', adjustable='box')
 xdata, ydata = [], []
 ln1, = plt.plot([], [], '-')
-ln2, = plt.plot([], [], 'go-', linewidth=2, markersize=5, c="black")
+ln2, = plt.plot([], [], 'o-', linewidth=2, markersize=5, c="black")
 ani = animation.FuncAnimation(fig, animate, x.shape[1], init_func=init, interval = param.dt * 500, blit= True, repeat = False)
 plt.show()
diff --git a/python/iLQR_manipulator.py b/python/iLQR_manipulator.py
index 1b10ac3f182a0b6685f55b29489a444a0b0f71ee..a95c4efc469621947304d539a510a6ff8b2f9712 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -31,8 +31,8 @@ import matplotlib.patches as patches
 def logmap(f, f0):
 	position_error = f[:2,:] - f0[:2,:]
 	orientation_error = np.imag(np.log(np.exp(f0[-1,:]*1j).conj().T * np.exp(f[-1,:]*1j).T)).conj()
-	error = np.vstack([position_error, orientation_error])
-	return error
+	diff = np.vstack([position_error, orientation_error])
+	return diff
 
 # Forward kinematics for end-effector (in robot coordinate system)
 def fkin(x, param):
@@ -95,11 +95,11 @@ param.nbPoints = 2 # Number of viapoints
 param.nbVarX = 3 # State space dimension (x1,x2,x3)
 param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3)
 param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation)
-param.l = [2,2,1] # Robot links lengths
-param.sz = [.2,.3] # Size of objects
+param.l = [2, 2, 1] # Robot links lengths
+param.sz = [.2, .3] # Size of objects
 param.r = 1e-6 # Control weight term
 param.Mu = np.asarray([[2, 1, -np.pi/6], [3, 2, -np.pi/3]]).T # Viapoints 
-param.A = np.zeros([2, 2,param.nbPoints]) # Object orientation matrices
+param.A = np.zeros([2, 2, param.nbPoints]) # Object orientation matrices
 param.useBoundingBox = True # Consider bounding boxes for reaching cost