From 45fce6653addee79718e37e52657199cc1b51f55 Mon Sep 17 00:00:00 2001
From: Sylvain CALINON <sylvain.calinon@idiap.ch>
Date: Mon, 4 Nov 2024 12:28:53 +0100
Subject: [PATCH] relative weight on orientation added in octave example

---
 matlab/iLQR_manipulator3D.m | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/matlab/iLQR_manipulator3D.m b/matlab/iLQR_manipulator3D.m
index ff616d5..c9570dc 100644
--- a/matlab/iLQR_manipulator3D.m
+++ b/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])); %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)
 
-- 
GitLab