I want to be able to model a circular mobility for robots.
Suppose we have 3 robots (the yellow ones) which should follow the green one. All of them move in circular fashion.
Calculating the positions and the direction:
Assuming that we know:
- the
(x,y)coordinates of the green robot - the orientation of the green robot
- the distance between the green robot and the yellow robot (the one in the middle)
- the distance between the yellow robots (dotted yellow line)
1) how can we calculate the (x,y) positions of the yellow robots?
2) how can we calculate the orientation of the yellow robots?
Or should we change the point of view, and try to calculate the positions and orientation of all of the robots assuming that we know a center-point x in the very middle of all of the circles?
There’s not quite enough information to compute the positions of the yellow robots. Let’s work backwards through this.
Let $d_{yy}$ be the spacing between the yellow robots, $d_{gy}$ be the distance between the green and middle yellow, $P_{g}=(x_{g},y_{g})$ be the position of the green robot, $P_{mid}=(x_{mid},y_{mid})$ the position of the middle yellow robot, and $\theta_g$ and $\theta_y$ their respective facings. An important thing to note is that the three yellow robots all lie on a single line and that this line passes through the common center of the circles.
If you know both the position and orientation of the middle yellow robot, then you can find the positions of the other two immediately: the line through them is perpendicular to their facing, so the outer robot’s position is $(x_{mid}-d_{yy}{\sin\theta_y},y_{mid}+d_{yy}\cos{\theta_y})$ and the inner robot’s position is $(x_{mid}+d_{yy}{\sin\theta_y},y_{mid}-d_{yy}\cos{\theta_y})$. If you know the position of one of the others instead, you can find the remaining two positions in a similar fashion.
Suppose instead that you know neither the position nor orientation of the yellows, but you do know the common center of the circles, $C=(x_c,y_c)$. Referring to the diagram above, you can see that $$\sin\alpha = {d_{gy}\over2|P_g-C|}.\tag{1}$$ Once you know this angle, you can find $P_{mid}$ in several ways. Assuming that we’re circling counterclockwise, one way is to rotate $P_g$ clockwise about $C$ through an angle of $2\alpha$. In matrix form, this rotation is $$R=\pmatrix{\cos{2\alpha}&\sin{2\alpha}\\-\sin{2\alpha}&\cos{2\alpha}}$$ and so $P_{mid}=C+R(P_g-C)$, i.e., $$\begin{align}x_{mid}&=x_c+(x_g-x_c)\cos{2\alpha}+(y_g-y_c)\sin{2\alpha}\\y_{mid}&=y_c-(x_g-x_c)\sin{2\alpha}+(y_g-y_c)\cos{2\alpha}\end{align}.$$ Note that you don’t actually have to compute the angle $\alpha$ explicitly in order to perform this rotation. You can use $\cos^2\alpha+sin^2\alpha=1$ and the double-angle identities for sine and cosine to express all of this in terms of $\sin\alpha$. The common yellow robot facing is $$\theta_y=\theta_g-2\alpha,\tag{2}$$ of course.
Another possibility, once you have the angle $\alpha$, is to find the direction vector of the line of yellow robots. The angle of this vector is the angle of the direction of $P_g-C$ less twice alpha, and the direction of $P_g-C$ is $\theta_{green}-\pi/2$ (again assuming we’re moving counterclockwise), so the direction of $P_{mid}-C$ is $\phi=\theta_g-\pi/2-2\alpha$. With this angle, we can find the positions of all three yellow robots: $(|P_g-C|-d_{yy})(\cos\phi,\sin\phi)$, $(|P_g-C|)(\cos\phi,\sin\phi)$ and $(|P_g-C|+d_{yy})(\cos\phi,\sin\phi)$.
Note that there’s an ambiguity here. There are in general two points on the circle that are at a given distance from $P_g$. Once corresponds to lagging behind the green robot by a lot, and will look like the green is following the yellows. You’ll need to use the green robot’s facing and some other information about the situation to decide which of these points to use.
If the robots are moving clockwise instead, just reverse all of those angles. You can determine the direction of motion by computing $$(x_g-x_c)\sin{\theta_g}-(y_g-y_c)\cos{\theta_g}$$ (this is basically the cross product of the two direction vectors). If this is positive, the robots are going counterclockwise; if negative, clockwise.
If you know $C$, but you have $\theta_y$ instead of $P_{mid}$, you can find $2\alpha$ from equation (2) and then proceed as before.
What if you don’t know where the center of the circle is? If you have $P_{mid}$, you can find it pretty easily. Referring to the previous diagram, you can see that $C$ is the intersection of the line through it and $P_g$ and the perpendicular bisector of $\overline{P_gP_{mid}}$. The green robot’s facing is perpendicular to this line, so you have enough information to work out the equations of the lines and find their intersection point.
If you only have the yellow robot’s facing $\theta_y$, finding $C$ is a bit more involved. We know that $C$ is at the intersection of three lines: the lines through the two robot positions that are perpendicular to their respective facings and the perpendicular bisector as before. We also know that $P_{mid}$ lies somewhere on the circle with radius $d_{gy}$ centered at $P_g$. If you put this together and work through the algebra, you’ll find that there are four possible solutions. You can then use the facings of the two robots to eliminate three of them, i.e., to decide on which side of the facing lines the center should be on. If you know that the robots are circulating in a particular direction, you can use that to zero in the the correct intersection point, too. If you examine the above diagram, you’ll probably find other geometric relationships that will let you find the circle’s center.