# Follow up question to Robot path planning

So I was browsing through the localization section and found this question and the code which answered it. https://robotics.stackexchange.com/a/7932/21145 but I have a follow-up question to this. So the code is exactly what I need but my "beacon" isn't in the middle but in the front (image following). Do I have to change the code or is the solution implemented general enough, I am not quite sure? The beacon is marked as point A.

In the other question, the beacon was being tracked by a camera to establish where the robot was located. I'm assuming your beacon is being used for the same purpose here.

I'm assuming that the distance from your robot's turning center $M$ and the beacon center $A$ is $r$, i.e. $r = \overline{MA}$.

You'll need to be tracking your robot's heading in order to do any of the path planning as described in the other answer, but now you're stuck with additional task of relating the measured beacon location back to the turning center location to be able to orient correctly to reach the target location(s).

This is fortunately a relatively straightforward task, but you need to be careful with the conventions you choose. Is your heading along the +y-axis, or the +x-axis? Does your heading angle increase clockwise or counterclockwise (anti-clockwise)?

I'm assuming here the y-axis points "up," x-axis points "right," the heading zero angle is aligned with the +y-axis, and heading angle increases CW. Refer to the diagram below:

This is where you need to be careful with the convention that you choose. It might not be the same as what I chose, but the method should be the same. You need to create a drawing that shows your choice of axes and take the same steps.

The easiest way to go about this is to put your axes centered on the turning center $M$, as I did in the graphic above. Next, find the relationship between your beacon location $A$ and the heading angle $\theta$. Given an $<x, y>$ coordinate scheme, at a heading of $\theta = 0$, the location of $A$ would be $<0, +r>$. If $\theta = 90^{\circ}$, then $A = <+r, 0>$.

This is a trig problem, and you can see now that you need $\mbox{Trig Function 1}$, or $\mbox{TF1}$ such that $\mbox{TF1}(0^{\circ}) = 0$ and $\mbox{TF1}(90^{\circ}) = +1$, then you need a second trig function $\mbox{TF2}(0^{\circ}) = +1$ and $\mbox{TF2}(90^{\circ}) = 0$.

Then you get:

$$A_x = M_x + TF1(\theta)*r \\ A_y = M_y + TF2(\theta)*r \\$$

This problem was simplified by putting the coordinate axes on $M$ such that $M_x = 0$ and $M_y = 0$. The trig functions that satisfy the criteria above are $\mbox{TF1} = \mbox{sine}$ and $\mbox{TF2} = \mbox{cosine}$. I'm not sure that there's a good way to intuitively explain how to determine this other than practice, learning the special values for trig functions, and then more practice (do all the homeworks!).

So, this means that you can find the coordinate of $A$ as:

$$A_x = M_x + \sin{(\theta)}*r \\ A_y = M_y + \cos{(\theta)}*r \\$$

You're not trying to find $A$, though, you're given $A$ and you're trying to find $M$! This is now straightforward - solve for $M$!

$$M_x = A_x - \sin{(\theta)}*r \\ M_y = A_y - \cos{(\theta)}*r \\$$

Again, as discussed in the other question to the other OP, you haven't described your method for determining heading, so I'm assuming it's just dead reckoning as described under the "Last Update" heading in the other answer.

The scope of your question seemed to be more about how you relate beacon location back to turning center location, so hopefully this covers it all in enough detail for you. Again, remember to do the same steps with your own coordinate system and reference datums - they might not be the same as what I've used!

• ty very much for your fast answer, so do I have to replace xc and yc with Ax−sin(θ)∗r and Ay−cos(θ)∗r ? I mean for the code. To determine the heading I will just put the robot on the x-axis so that the heading is 0°. Yeah the beacon is being tracked by a PLT 300 it's a totalstation. – Sentrinity Sep 14 at 6:24
• And if point was for example not perpendicular to the connection line between but in a 45° so I would just have to change θ then or do I have to do other stuff? – Sentrinity Sep 14 at 12:23
• @Sentrinity - If you will just put the robot on the x-axis so that the heading is 0°, then at $\theta = 0$ you have $A = <+r, 0>$. This is different than the example I gave where the example heading I used was zero degrees when aligned to the y-axis. Directionality matters, too - are you using a mathematical convention of positive counter-clockwise, or are you using a navigational convention of positive clockwise? – Chuck Sep 14 at 12:45
• Regarding your second point, I'm assuming you mean that the direction from $M$ to the beacon $A$ is not along the same line that defines the forward vector. Is this correct? In that case you will again need to define your reference frames, then add the beacon angular offset to your heading, such that $\theta_{\mbox{effective}} = \theta_{\mbox{beacon angular offset}} + \theta_{\mbox{heading}}$. The angular offset would be the angle that $\overline{MA}$ makes with your forward vector, as defined using the same convention you're using for the heading! – Chuck Sep 14 at 12:49
• Yeah I meant that the direction from M to the beacon A is not the same line that defines the forward vector. I don't get what you mean with mathematical convention or navigational convention could you rephrase that? – Sentrinity Sep 14 at 13:08