matlab/iLQR_manipulator3D.m

@@ -33,9 +33,9 @@ param.dh.d = [0.333, 0, 0.316, 0, 0.384, 0, 0, 0.107]; %Offset along previous z
 param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0]; %Length of the common normal 
 
 %Weight matrices for iLQR cost
-%Q = speye((param.nbVarF-1) * param.nbPoints) * param.q; %Precision matrix
-%Q = kron(eye(param.nbPoints), diag([0, 0, 0, 1E0, 1E0, 1E0])); %Precision matrix (by removing position constraint)
-Qr = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])); %Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
+%Qr = speye((param.nbVarF-1) * param.nbPoints) * param.q; %Precision matrix in relative coordinate frame (tool frame)
+%Qr = kron(eye(param.nbPoints), diag([0, 0, 0, 1E0, 1E0, 1E0])); %Precision matrix in relative coordinate frame (tool frame), by removing position constraint
+Qr = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])) * param.q; %Precision matrix in relative coordinate frame (tool frame), by removing orientation constraint on 3rd axis
 
 R = speye(param.nbVarU * (param.nbData-1)) * param.r; %Control weight matrix (at trajectory level)
 

python/IK_manipulator3D.py

@@ -125,7 +125,7 @@ def R2q(R):
 			[R[1,2]-R[2,1], R[2,0]-R[0,2], R[0,1]-R[1,0], R[0,0]+R[1,1]+R[2,2]],
 	]) / 3.0
 
-	e_val, e_vec = np.linalg.eig(K)
+	e_val, e_vec = np.linalg.eig(K) # unsorted eigenvalues
 	q = np.real([e_vec[3, np.argmax(e_val)], *e_vec[0:3, np.argmax(e_val)]]) # for quaternions as [w,x,y,z]
 	return q
 

python/iLQR_manipulator3D.py

@@ -120,9 +120,7 @@ def R2q(R):
 			[R[2,0]+R[0,2], R[2,1]+R[1,2], R[2,2]-R[0,0]-R[1,1], R[0,1]-R[1,0]],
 			[R[1,2]-R[2,1], R[2,0]-R[0,2], R[0,1]-R[1,0], R[0,0]+R[1,1]+R[2,2]],
 	]) / 3.0
-
-	e_val, e_vec = np.linalg.eig(K)
-	# /!\ With numpy eigenvalues are not sorted!
+	e_val, e_vec = np.linalg.eig(K) # unsorted eigenvalues
 	q = np.real([e_vec[3, np.argmax(e_val)], *e_vec[0:3, np.argmax(e_val)]]) # for quaternions as [w,x,y,z]
 	return q
 
@@ -172,8 +170,7 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
 
 # Precision matrix
 # Q = np.identity((param.nbVarF-1) * param.nbPoints) # Full precision matrix
-Qr = np.diag( [1.0,1.0,1.0,1.0,1.0,0.0] * param.nbPoints) # Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
-
+Qr = np.diag([1.0,1.0,1.0,1.0,1.0,0.0] * param.nbPoints) # Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
 
 # Control weight matrix (at trajectory level)
 R = np.eye((param.nbData-1) * param.nbVarU) * param.r
@@ -196,7 +193,7 @@ Su = Su0[idx,:] # We remove the lines that are out of interest
 # ===============================
 
 u = np.zeros((param.nbVarU * (param.nbData-1), 1)) # Initial control command
-x0 = np.array([0, 0, 0, -np.pi/2, -0, np.pi/2, 0]) # Initial robot pose
+x0 = np.array([0, 0, 0, -np.pi/2, 0, np.pi/2, 0]) # Initial robot pose
 
 x0 = x0.reshape((-1, 1))
 
@@ -219,10 +216,10 @@ for i in range(param.nbIter):
 
 	Q = Ra @ Qr @ Ra.T # Precision matrix in absolute coordinate frame (base frame)
 	
-	#du = np.linalg.pinv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
+	#du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f - u * param.r) # Gauss-Newton update
 	du = np.linalg.lstsq(
 		Su.T @ J.T @ Q @ J @ Su + R,
-		-Su.T @ J.T @ Q @ f - u * param.r ,
+		-Su.T @ J.T @ Q @ f - u * param.r,
 		rcond=-1
 	)[0] # Gauss-Newton update