﻿ RCFS packages = ['numpy']

# Exercise 7Iterative linear quadratic regulator

In Exercise 6a, we have seen how LQR can be used to control a point-mass agent to pass through a set of viapoints.

In Exercise 6b, we have seen how LQR can be used to control a robot with joint angle velocity commands to pass through a set of joint angle configurations.

In this exercise we will now see how an iterative linear quadratic regulator (iLQR) can be used to control a robot with joint angle velocity commands

We will consider a combination of viapoints in both joint space and task space. The goal will be to have the robot passing through a set of intermediate joint angle configurations and to have its end-effector passing through another set a intermediate viapoints.

The first viapoint param.viapoint1 is defined in the joint space and visualized by the orange robot.

The second viapoint param.viapoint2 is defined in the task space and visualized by the pink object.

The third viapoint param.viapoint3 is defined in the task space and visualized by the green object.

The goal of this exercise is to plan a path with iLQR that generates a motion passing through a combination of task space and joint space viapoints.

We consider a single integrator dynamical system with $\bm{x}_t$ and $\bm{u}_t$ representing joint angles and velocity commands at time step $t$, respectively.

The cost function at a given time step $t$ is defined by \begin{equation*} c(\bm{x}_t,\bm{u}_t) = \sum_{i=1}^3\bm{f}_i(\bm{x}_t)^{\!\trsp} \bm{Q}_{i,t} \bm{f}_i(\bm{x}_t) + \bm{u}_t^\trsp \bm{R}_t \, \bm{u}_t, \end{equation*} where $\bm{Q}_{i,t}$ and $\bm{R}_t$ are weight matrices trading off viapoints tracking and control effort. Such cost is quadratic on $\bm{f}^i(\bm{x}_t)$ but can be non-quadratic on $\bm{x}_t$, which requires iLQR to be used instead of LQR.

The solution to this problem results in the following update of the control commands at each iteration of iLQR \begin{equation*} \Delta\bm{\hat{u}} \!=\! {\Bigg(\bm{S}_{\bm{u}}^\trsp \sum_{i=1}^3\Big(\bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{J}_i(\bm{x})\Big) \bm{S}_{\bm{u}} \!+\! \bm{R}\Bigg)}^{\!\!-1} \Bigg(- \bm{S}_{\bm{u}}^\trsp \sum_{i=1}^3\Big(\bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{f}_i(\bm{x})\Big) - \bm{R} \, \bm{u} \Bigg). \end{equation*}

• Change the code in the compute_fJ() function so that the robot can pass through the set of viapoints defined in joint space and task space. You can verify your answer by moving the objects and changing the joint angle configurations of the robot for both initial pose (in light gray) and viapoint pose (in orange).
• Move the objects and modify the parameters tl, Q1, Q2 and Q3. Observe the resulting behaviors and the resulting cost.

# Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * 1E-6 # Precision matrices Q1 = np.eye(param.nbVarX) Q2 = np.eye(param.nbVarF) Q3 = np.eye(param.nbVarF) # Time occurrence of viapoints tl = np.array([17, 34, 49]) # # Joint angle configuration at the first viapoint # print(x[:,tl]) # # End-effector position at the first viapoint # print(fkin(x[:,tl], param)) # print(param.viapoint1) #First viapoint # print(param.viapoint2) #Second viapoint # print(param.viapoint3) #Third viapoint def compute_fJ(x, param): # Residual when reaching the first viapoint (in joint space, orange robot) f1 = np.zeros(param.nbVarX) # To be replaced # Residual when reaching the second viapoint (in task space, pink object) f2 = np.zeros(param.nbVarF) # To be replaced # Residual when reaching the third viapoint (in task space, green object) f3 = np.zeros(param.nbVarF) # To be replaced # Jacobian of the residual when reaching the first viapoint (in joint space, orange robot) J1 = np.zeros((param.nbVarX, param.nbVarX)) # To be replaced # Jacobian of the residual when reaching the second viapoint (in task space, pink object) J2 = np.zeros((param.nbVarF, param.nbVarX)) # To be replaced # Jacobian of the residual when reaching the third viapoint (in task space, green object) J3 = np.zeros((param.nbVarF, param.nbVarX)) # To be replaced return f1,f2,f3,J1,J2,J3 update_iLQR()
# Control weight matrix R = np.identity((param.nbData-1) * param.nbVarU) * 1E-6 # Precision matrix Q1 = np.eye(param.nbVarX) Q2 = np.eye(param.nbVarF) Q3 = np.eye(param.nbVarF) # Time occurrence of viapoints tl = np.array([17, 34, 49]) def compute_fJ(x, param): # Viapoint residuals f1 = x[:,tl] - param.viapoint1 f2 = fkin(x[:,tl], param) - param.viapoint2 f3 = fkin(x[:,tl], param) - param.viapoint3 # Corresponding Jacobians J1 = np.eye(param.nbVarX) J2 = Jkin(x[:,tl], param) J3 = Jkin(x[:,tl], param) return f1,f2,f3,J1,J2,J3 update_iLQR()

Cost: