I want to simulate the detection of a moving object by a unicycle type robot. The robot is modelled with position (x,y) and direction theta as the three states. The obstacle is represented as a circle of radius r1 (r_1
in my code). I want to find the angles alpha_1
and alpha_2
from the robot's local coordinate frame to the circle, as shown here:
So what I am doing is trying to find the angle from the robot to the line joining the robot and the circle's centre (this angle is called aux_t
in my code), then find the angle between the tangent and the same line (called phi_c
). Finally I would find the angles I want by adding and subtracting phi_c
from aux_t
. The diagram I am thinking of is shown:
The problem is that I am getting trouble with my code when I try to find the alpha angles: It starts calculating the angles correctly (though in negative values, not sure if this is causing my trouble) but as both the car and the circle get closer, phi_c
becomes larger than aux_t
and one of the alphas suddenly change its sign. For example I am getting this:
$$\begin{array}{c c c c} \text{aux_t} & \text{phi_c} & \text{alpha_1} & \text{alpha_2} \\ \hline \text{-0.81} & \text{+0.52} & \text{-1.33} & \text{-0.29} \\ \text{-0.74} & \text{+0.61} & \text{-1.35} & \text{-0.12} \\ \text{-0.69} & \text{+0.67} & \text{-1.37} & \text{-0.02} \\ \text{-0.64} & \text{+0.74} & \text{-1.38} & \text{+0.1} \\ \end{array}$$
So basically, the alpha_2
gets wrong form here. I know I am doing something wrong but I'm not sure what, I don't know how to limit the angles from 0 to pi. Is there a better way to find the alpha angles?