Previously, in exercises 4 and 5, we defined a desired target position \bm{x}_{\text{target}} defined either in joint space or task space and we wanted the robot to reach this target position. We did not specify the timings or the viapoints/paths that the robot should follow to reach these target positions.
Optimal control is the principled way of planning a robot motion by specifying timings and/or a set of viapoints or a reference trajectory to follow.
Linear quadratic regulator (LQR) is the most simple form of optimal control. In this exercise, the goal is to understand how LQR works and how it can be applied to robot planning problems. You can read more about this in Section 7 of the RCFS documentation.
This exercise allows you to specify the precision matrices used in LQR and to change the placement of two viapoints. The series of control commands generates a resulting path that can be visualized on the right figure (the black point shows the initial position). The graphs show the corresponding covariance matrices (inverses of precision matrices) for two viapoints (in pink and green).
param.nbDeriv=2
so that the point-mass system is controlled with acceleration commands instead of velocity commands (system dynamics defined as a double integrator instead of a simple integrator). Set param.r
to a higher value until you see that it does not reach the pink line anymore. Set param.nbDeriv=1
again to compare the smoothness of the two trajectories.