I am working on controlling a robot to various points on an x,y grid. The robot starts at random locations and I can control it's velocity and angular velocity.

Currently, I am using the technique of rotate the robot until the atan2 of the distance between the robot and the target goal like so:

dx = target_position.x - robot_position.x
dy = target_position.y - robot_position.y
angle = atan2(dy, dx)
angle = wrap_to_pi(angle) # Wrap the angles to the range [-π, π] radians.

if angle - robot_angle <= some_error_factor:
   increase_linear_velocity() # note this is pseudo code

This works pretty well to get me to target positions when the robot never has to go near x=0 on the coordinate system. I know there is a discontinuity with atan2 near pi, but my question is how can I wrap the angle or control the robot because currently when x is close to 0 the robot just spins around unable to get under the error factor to say you're pointing at the position

  • $\begingroup$ The error on the angle shall have control on the angular velocity, not on the linear velocity. $\endgroup$ Feb 5 '21 at 17:34
  • 1
    $\begingroup$ Sorry it's not the full code. But basically it is set up that the robot will spin until the difference of the desired angle and the angle of the robot is lower than the tolerance and if it is stop rotating and apply a linear velocity $\endgroup$ Feb 5 '21 at 17:51

First off, your code may have some confusion about atan2, which already returns an angle between -pi and +pi. That's one reason to use atan2(dy, dx) instead of atan(dy/dx) -- it figures out the correct quadrant so we don't have to. (The other reason to use atan2 is that it works fine when dx is near zero, where atan(dy/dx) will suffer loss of precision, overflow, or even division by zero.)

Secondly, I think your code is doing the pi-wrap in the wrong place. Try something more like this:

dx, dy = target_position - robot_position
target_angle = atan2(dy, dx)
d_angle = target_angle - robot_angle
d_angle = wrap_to_p(d_angle)    # wrap the angle here
if abs(d_angle) < acceptable_error
elif d_angle < 0:
   turn left some
   turn right some
  • $\begingroup$ I definitely was confused about atan2. I will remove the wrapping because it doesn't seem to be necessary. I think the problem is that when I rotate the bot it can go clockwise or counter clockwise depending on which way is faster, but when near 0,0 for some reason it never settles on a direction because it over corrects continually. I could potentially modify how quickly I rotate or the acceptable error value, but I was wondering if this was more of an issue of don't use atan around origin or something $\endgroup$ Feb 6 '21 at 2:06
  • $\begingroup$ If the dx,dy is 0,0 that means the robot is at the target. The hunting is because there is no direction the robot can go to get closer. Of course, there is no need to go at all. Change the code to first check (dx**2 + dy**2 < acceptable_position_error**2) and if so, stop. Otherwise, do the rest of the stuff above. $\endgroup$
    – r-bryan
    Feb 9 '21 at 23:30

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.