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*}
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).