This exercise extends the previous example with an optimal control problem in joint space. You can specify the precision matrices used in LQR, change the poses of two viapoints, and then visualize the resulting motion found by LQR.
Change the code in the LQR in joint space tab so that the first viapoint is a 'picking configuration' (pink object) and the second viapoint is a 'placing configuration' (green object).
param.nbVarPos = 3 # Dimension of position
param.nbDeriv = 1 # Number of derivatives, 2 for acceleration control
param.nbVarX = param.nbVarPos*param.nbDeriv # Dimension of the state space
param.r = 1e-2 # precision of the control command
Sx, Su = compute_transfer_matrices(param)
# precision matrix of the first viapoint
Q1 = np.eye(param.nbVarX)
# precision matrix of the second viapoint
Q2 = np.eye(param.nbVarX)
param.Mu[:,0] = np.array([-np.pi/3, np.pi/2, np.pi]) # 1st viapoint
param.Mu[:,1] = np.array([-np.pi/2, np.pi/2, np.pi]) # 2nd viapoint
# Time occurrence of viapoints
tl = np.array([24, 49])
idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl*param.nbVarX)])
x = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial state