FD compatibility improved

python/FD.py

@@ -81,9 +81,9 @@ Tm = np.multiply(np.triu(np.ones([m.shape[1], m.shape[1]])), np.repeat(m, m.shap
 
 # Initialization
 # ===============================
-x0 = np.zeros([2*param.nbVarX, 1]) #initial states
+x = np.zeros([2*param.nbVarX, 1]) #initial states
 u = np.zeros([param.nbVarU*(param.nbData - 1), 1]) #Input commands for the whole task.  
-x = x0
+
 # Forward Dynamics
 for t in range(param.nbData - 1):
     # Computation in matrix form of G,M, and C
@@ -96,10 +96,11 @@ for t in range(param.nbData - 1):
     # Compute acceleration
     tau = np.reshape(u[(t) * param.nbVarX:(t+1)*param.nbVarX], [param.nbVarX, 1])
     inv_M = np.linalg.inv(M)
-    ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1]))**2) - T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1])*param.damping
+    ddq = inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1]))**2) - \
+          T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1])*param.damping
 
     # compute the next step
-    xt = x[:,t].reshape(2 * param.nbVarX, 1) + np.vstack([x[param.nbVarX:, t].reshape(param.nbVarX, 1),ddq]) * param.dt
+    xt = x[:,t].reshape(2 * param.nbVarX, 1) + np.vstack([x[param.nbVarX:, t].reshape(param.nbVarX, 1), ddq]) * param.dt
     x = np.hstack([x, xt])
 
 # Plot

python/FD_recursive.py

@@ -29,23 +29,17 @@ import matplotlib.animation as animation
 # Helper functions
 # ===============================
 
+# 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 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
-
 # Initialization of the plot
 def init():
     global param
@@ -53,12 +47,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 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
 
 
@@ -86,9 +81,9 @@ Tm = np.multiply(np.triu(np.ones([m.shape[1], m.shape[1]])), np.repeat(m, m.shap
 
 # Initialization
 # ===============================
-x0 = np.zeros([2*param.nbVarX, 1]) #initial states
+x = np.zeros([2*param.nbVarX, 1]) #initial states
 u = np.zeros([param.nbVarU*(param.nbData - 1), 1]) #Input commands for the whole task.  
-x = x0
+
 # Forward Dynamics
 for t in range(param.nbData - 1):
         
@@ -109,7 +104,8 @@ for t in range(param.nbData - 1):
     # Compute acceleration
     tau = np.reshape(u[(t) * param.nbVarX:(t+1)*param.nbVarX], [param.nbVarX, 1])
     inv_M = np.linalg.inv(M)
-    ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1]))**2) - T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1])*param.damping
+    ddq = inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1]))**2) - \
+          T @ np.reshape(x[param.nbVarX:, t],[param.nbVarX, 1])*param.damping
 
     # compute the next step
     xt = x[:,t].reshape(2 * param.nbVarX, 1) + np.vstack([x[param.nbVarX:, t].reshape(param.nbVarX, 1),ddq]) * param.dt
@@ -122,6 +118,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 * 1000, blit= True, repeat = False)
 plt.show()