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 in task space.
The goal of this exercise is to plan a path with iLQR to generate a motion passing through this combination of task space and joint space viapoints:
param.viapoint1 is defined in the joint space, displayed as the orange robot.param.viapoint2 is defined in the task space, displayed as the pink object.param.viapoint3 is defined in the task space, displayed as the green object.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 given 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 the residual functions $\bm{f}_i(\bm{x}_t)$ but nonquadratic 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(\sum_{i=1}^3 \bm{S}_{\bm{u},i}^\trsp \bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{J}_i(\bm{x}) \bm{S}_{\bm{u},i} \!+\! \bm{R}\Bigg)}^{\!\!-1} \Bigg(- \sum_{i=1}^3 \bm{S}_{\bm{u},i}^\trsp \bm{J}_i(\bm{x})^\trsp \bm{Q}_i \bm{f}_i(\bm{x}) - \bm{R} \, \bm{u} \Bigg), \end{equation*} where $\bm{J}_i(\bm{x})$ is the Jacobian matrix of $\bm{f}_i(\bm{x})$.
compute_fJ() 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).