Differences

This shows you the differences between two versions of the page.

--- turtlebot:adventures:sensing101_forwarderror [2015/10/06 23:50] – [Complex Matrices in Python] pvela
+++ turtlebot:adventures:sensing101_forwarderror [2024/08/20 21:38] (current) – external edit 127.0.0.1
@@ Line 42: / Line 42: @@
 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.
+==== Generating the Desired End Pose ====
+Alright, the above is all fine and good, but the bigger question is: how do I create the desired pose signal to begin with?  It actually works in the opposite way as computing the error.  Let's consider the "error" to be a difference denoted by $\Delta g$,
+\begin{equation}
+  \Delta g = g_{curr}^{-1} g_{des}.
+\end{equation}
+Now, let's rearrange it to solve for $g_{des}$,
+\begin{equation}
+  g_{des} = g_{curr} \Delta g.
+\end{equation}
+Ha!  This final equation is exactly what you want to do.  Take the current position, as measured prior to actually performing any feedback, and left-multiply by the desired movement of the Turtlebot.  For pure forward motion and for pure turning, this would be:
+\begin{equation}
+  \Delta g_{forward} = \left[ \begin{matrix} 1 & \Delta x + 0j \\ 0 & 1 \end{matrix} \right]
+  \quad \text{and} \quad
+  \Delta g_{turn} = \left[ \begin{matrix} e^{j \Delta \theta} & 0 \\ 0 & 1 \end{matrix} \right],
+\end{equation}
+where $\Delta x$ is the distance to drive forward and $\Delta \theta$ is the angle (in radians most likely) to turn.  In future instantiations, we'll probably do more complex relative pose changes which involve both moving to an arbitrary point relative to the robot (having both x and y offsets), plus an arbitrary angle.
+This desired end pose will then be fixed until the proper terminating condition is achieved (e.g., the error is small enough to be negligible).
 ==== Complex Matrices in Python ====
@@ Line 51: / Line 70: @@
 After that you will be happy because then complex matrices will be supported. Let's build the simplest such matrix, which is the identity matrix of $SE(2)$.  It is:
 <code>
-> r = rect(1,0)
+> r = cmath.rect(1,0)
 > t = complex(0,0)
 > g = numpy.mat([[r, t], [0, 1]], dtype=complex)
@@ Line 58: / Line 77: @@
 If I am not mistaken, you should now have the identity matrix!  That's boring you say, you want a real matrix.  Ok then let's have the robot rotated by $30^\circ$ and translated by 5 units in the $x$ and negative 7 in the $y$,
 <code>
-> r = rect(1,numpy.pi/6)
+> r = cmath.rect(1,numpy.pi/6)
 > t = complex(5,-7)
 > g = numpy.mat([[r, t], [0, 1]], dtype=complex)
 > g
 </code>
-Taking the inverse you should see that the top right part of the matrix is conjugated:
+I recommend using ''cmath.rect'' for the rotation complex number because the arguments are the magnitude of the complex number (for rotations, it is always 1) and then the phase of the complex number (which is the orientation we wish to achieve).
+Taking the inverse you should see that the top left part of the matrix gets conjugated:
 <code>
 > gInverse = numpy.linalg.inv(g)
@@ Line 72: / Line 92: @@
 > gErr = gCurrInv * gDes
 </code>
+or maybe even
+<code>
+> gErr = numpy.linalg.inv(gCurr) * gDes
+</code>
+There is actually a computationally faster way of doing it that exploits the fact that $r$ is a rotation, but that's for another day, or maybe even for you to explore on your own.
 ---------
 ;#;
 [[Turtlebot:Adventures:Sensing101 | Sensing 101]]
 ;#;