From a2e011633b4cb1d8025a5352a9d718296ff32e7c Mon Sep 17 00:00:00 2001 From: Sylvain CALINON <sylvain.calinon@idiap.ch> Date: Mon, 4 Nov 2024 17:20:50 +0100 Subject: [PATCH] relative weight on orientation added in octave example --- matlab/iLQR_manipulator3D.m | 2 +- python/iLQR_manipulator3D.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/matlab/iLQR_manipulator3D.m b/matlab/iLQR_manipulator3D.m index c9570dc..a14ac70 100644 --- a/matlab/iLQR_manipulator3D.m +++ b/matlab/iLQR_manipulator3D.m @@ -35,7 +35,7 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0]; %Length of the common norm %Weight matrices for iLQR cost %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])); %Precision matrix in relative coordinate frame (tool frame), by removing orientation constraint on 3rd axis +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) diff --git a/python/iLQR_manipulator3D.py b/python/iLQR_manipulator3D.py index a49b92c..45b255f 100644 --- a/python/iLQR_manipulator3D.py +++ b/python/iLQR_manipulator3D.py @@ -171,7 +171,6 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm # 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) - # Control weight matrix (at trajectory level) R = np.eye((param.nbData-1) * param.nbVarU) * param.r @@ -193,7 +192,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)) -- GitLab