By now you've been able to do orientation control. The current adventure now requests that you add a little forward position control. What will be described here is yet another step towards full fledged geometric control of the Turtlebot. The geometric part means that we will try to use the natural geometry of the Turtlebots space to create feedback controls that look like what you are used to. There will be some nonlinear math going on, but we are going to hide it all so that things look like the standard error feedback that you are used to. That's what makes it geometric. Some systems require actual nonlinear controls that really look nonlinear. The Turtlebot moves in what is called the special Euclidean plane, written as $SE(2)$.

The special Euclidean plane consists of the translational position of the robot and the orientation of the robot. You've seen it packed into the pose as a quaternion and as a translation vector (in 3D). Both of these are for representing the full special Euclidean space, written as $SE(3)$. The Turtlebot can't (or shouldn't rather) move in this full 3D space, so we have been restricting it to the 2D (or planar) version. That's what we will do here. The nice thing about our geomoetric laws, is that if you get them, then you will automatically get how to do them for more complex 3D robots too!

What we are going to do is to define the translation and the orientation as a complex 2×2 matrix. This will be done by ripping out the complex part of the quaternion and putting that into a complex number $r \in \mathbb{C}$ just as for the turn control case. Then, take out the translation part and dump that into a complex number $t \in \mathbb{C}$. Then pack them together into a complex 2×2 matrix, that we will label $g$, as follows: \begin{equation} g = \left[ \begin{matrix} r & t \\ 0 & 1 \end{matrix} \right] \end{equation} This matrix defines the position and orientation of the Turtlebot. One thing we did not discuss explicitly is that $r \in \mathbb{C}$ is actually unit length. As a rotation it should be. Being unit length means that the inverse of $r$ is actually the complex conjugate since $r r^* = 1$ (by virtue of being unit length). This can be useful to know for speeding up certain operations.

The complex matrix form of $SE(2)$ lets us compute the error just like for the complex orientation case. Suppose that you have constructed the complex matrices $g_{des}$ and $g_{curr}$ properly, then the error is \begin{equation} g_{err} = g_{curr}^{-1} g_{des} = \left[ \begin{matrix} r_{err} & t_{err} \\ 0 & 1 \end{matrix} \right] \end{equation} Pulling out $r_{err}$ gives the orientation error as before \begin{equation} \theta_{err} = \text{phase}(r_{err}) \end{equation} Pulling out the real part of $t_{err}$ gives the forward movement error \begin{equation} x_{err} = \text{Re}(t_{err}) \end{equation} We won't use it for now, but the imaginary part of $t_{err}$ gives the sideways error. Since the Turtlebot can't move sideways, we will ignore it for now. To actually be able to modify the $y_{err}$ through a control signal requires more advanced concepts from control theory. These will show up in a future Adventure.

The orientation control signal gets computed just like before \begin{equation} \omega = k_{\theta} \theta_{err} \end{equation} while the forward drive control signal gets computed as \begin{equation} v = k_{drive} x_{err}. \end{equation} where $v$ specifies the forward velocity. As in the goforward and other related python code the forward velocity is the first component of the twist. In ROS that would be the twist.linear.x field within the twist. As usual you should be applying a saturation function to the controls so that they don't give crazy big values. What would be a reasonable upper bound on the Turtlebot's speed?

If your goal here is to go to a static Turtlebot pose, then you should have some kind of stop-control condition that checks to see if the two errors are too small to bother fixing. I think I had used something like one half or one quarter of a degree for the orientation control back in the earlier adventure. For the forward error, I am not sure what I would use. I'd test thing out by going to some older code that published the pose (hopefully you have such a test code) and check how little I could move the Turtlebot and still see a change in its position. The limit should then be set to be a little higher than this since that value is the resolution or granularity of the sensor. You won't get better than that. I had done something similar for the orientation control to arrive at my control limit.

Sensing 101