Differences

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

--- turtlebot:adventures:sensing101_forwarderror [2015/10/06 23:26] – pvela
+++ turtlebot:adventures:sensing101_forwarderror [2024/08/20 21:38] (current) – external edit 127.0.0.1
@@ Line 41: / Line 41: @@
 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 ====
+OK, so you are thinking "Argh!!! How do I do this all in python????"  Well, fear not, because I went through the same painful experience and have some tips on one way to solve things.  First of all, you will definitely need ''numpy'' and quite possibly ''cmath'' (if it is not imported as part of numpy),
+<code>
+> import numpy
+> import cmath
+</code>
+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 = cmath.rect(1,0)
+> t = complex(0,0)
+> g = numpy.mat([[r, t], [0, 1]], dtype=complex)
+> g
+</code>
+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 = cmath.rect(1,numpy.pi/6)
+> t = complex(5,-7)
+> g = numpy.mat([[r, t], [0, 1]], dtype=complex)
+> g
+</code>
+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)
+> gInverse
+</code>
+If you can get the above working, then that's pretty much all you will need.  Matrix multiplication should work how you think it does:
+<code>
+> 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]]
 ;#;