Previously, in exercises 4 and 5, a desired target position was defined either in joint space or task space, without specifying the timings when the robot should follow to reach these target positions. We here extend the optimization to problems in which we can describe the timings and consider viapoints or full reference paths to be tracked, which provides the robot with planning and anticipation capability.
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.
This exercise considers a point-mass agent in a 2D space. The two viapoints can be moved with the mouse. The provided code snippet 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 displayed in the figure (the black point shows the initial position). The figure also shows the corresponding covariance matrices (inverses of precision matrices) for the two viapoints (in pink and green).
Q1
. Move the green point and observe the resulting path. Set Q1
back to an identity matrix.
param.nbDeriv=2
so that the point-mass agent is controlled with acceleration commands instead of velocity commands (system dynamics defined as a double integrator instead of a simple integrator).Q1
and Q2
, set precision matrices so that the first keypoint can be reached with any velocity and the final keypoint is reached with null velocity.Q1
and Q2
to change the desired precision requested to pass through the two viapoints and observe the results.
param.r
and observe the result on the path.