This exercise extends the previous example with an optimal control problem in joint space. I the code snippet, you can specify the precision matrices used in LQR, change the positions of two viapoints.
Modify the code so that the robot first reach the joint angle configuration displayed in pink (viapose) and then move to the viapose in green.
param.nbVarPos = 3 # Dimension of position variable (here, joint angles)
param.nbDeriv = 1 # Number of derivatives (1 for velocity commands)
param.nbVarX = param.nbVarPos * param.nbDeriv # Dimension of the state space
param.r = 1E-4 # Control command weight
x = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial state
param.Mu[:,0] = x # 1st viapoint position
param.Mu[:,1] = x # 2nd viapoint position
# Time occurrence of viapoints
tl = np.array([50, 100])
# Precision matrix for the first viapoint
Q1 = np.eye(param.nbVarX)
# Precision matrix for the second viapoint
Q2 = np.eye(param.nbVarX)
xs = solve_LQR(x, param) #Compute trajectory
param.nbVarPos = 3 # Dimension of position variable (here, joint angles)
param.nbDeriv = 1 # Number of derivatives (1 for velocity commands)
param.nbVarX = param.nbVarPos * param.nbDeriv # Dimension of the state space
param.r = 1E-4 # Control command weight
x = np.array([-np.pi/4, np.pi/2, np.pi/4]) # Initial state
param.Mu[:,0] = np.array([-np.pi/2, np.pi/2, np.pi/2]) # 1st viapoint position
param.Mu[:,1] = np.array([np.pi/3, np.pi/3, np.pi/3]) # 2nd viapoint position
# Time occurrence of viapoints
tl = np.array([50, 100])
# Precision matrix for the first viapoint
Q1 = np.eye(param.nbVarX)
# Precision matrix for the second viapoint
Q2 = np.eye(param.nbVarX)
xs = solve_LQR(x, param) #Compute trajectory