Skip to content
Snippets Groups Projects
Commit a987434d authored by Jérémy MACEIRAS's avatar Jérémy MACEIRAS
Browse files

feat: allow custom Q matrix

parent 0c15ba09
No related branches found
No related tags found
No related merge requests found
...@@ -144,7 +144,7 @@ param.nbPoints = 2 # Number of viapoints ...@@ -144,7 +144,7 @@ param.nbPoints = 2 # Number of viapoints
param.nbVarX = 7 # State space dimension (x1,x2,x3,...) param.nbVarX = 7 # State space dimension (x1,x2,x3,...)
param.nbVarU = param.nbVarX # Control space dimension (dx1,dx2,dx3,...) 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.nbVarF = 7 # Task space dimension (f1,f2,f3 for position, f4,f5,f6,f7 for unit quaternion)
param.q = 1e0 # Tracking weighting term 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.r = 1e-6 # Control weighting term param.r = 1e-6 # Control weighting term
Rtmp = q2R([np.cos(np.pi/3), np.sin(np.pi/3), 0.0, 0.0]) Rtmp = q2R([np.cos(np.pi/3), np.sin(np.pi/3), 0.0, 0.0])
...@@ -170,7 +170,8 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm ...@@ -170,7 +170,8 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0] # Length of the common norm
# =============================== # ===============================
# Precision matrix # Precision matrix
Q = np.eye((param.nbVarF-1) * param.nbPoints) * param.q Q = np.diag( param.q * param.nbPoints)
# Control weight matrix (at trajectory level) # Control weight matrix (at trajectory level)
R = np.eye((param.nbData-1) * param.nbVarU) * param.r R = np.eye((param.nbData-1) * param.nbVarU) * param.r
...@@ -204,7 +205,7 @@ for i in range(param.nbIter): ...@@ -204,7 +205,7 @@ for i in range(param.nbIter):
f, J = f_reach(x[:,tl], param) # Residuals and Jacobians f, J = f_reach(x[:,tl], param) # Residuals and Jacobians
f = f.reshape((-1,1), order='F') f = f.reshape((-1,1), order='F')
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
# Estimate step size with backtracking line search method # Estimate step size with backtracking line search method
alpha = 1 alpha = 1
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment