Merge branch 'master' of gitlab.idiap.ch:rli/robotics-codes-from-scratch

python/iLQR_manipulator3D.py

@@ -13,8 +13,7 @@ License: GPL-3.0-only
 import numpy as np
 import numpy.matlib
 import matplotlib.pyplot as plt
-import matplotlib.patches as patches
-import math
+
 
 # Helper functions
 # ===============================
@@ -144,7 +143,7 @@ param.nbPoints = 2 # Number of viapoints
 param.nbVarX = 7 # State space dimension (x1,x2,x3,...)
 param.nbVarU = param.nbVarX # Control space dimension (dx1,dx2,dx3,...)
 param.nbVarF = 7 # Task space dimension (f1,f2,f3 for position, f4,f5,f6,f7 for unit quaternion)
-param.q = [1,1,1,0.1,0.1,0.1] # Tracking weighting term for each parameter of F (3 first position, 3 last orientation)
+param.q = [1,1,1,1,1,1] # Tracking weighting term for each parameter of F (3 first position, 3 last orientation)
 param.r = 1e-6 # Control weighting term
 
 Rtmp = q2R([np.cos(np.pi/3), np.sin(np.pi/3), 0.0, 0.0])