How to calculate orientation of turtlebot given desired Vx and Vy velocities, goal and start position?

asked 2021-03-16 16:10:17 -0500

Nagarjun gravatar image

Hi Guys, I am new to ros. Can anyone let me know answer to this problem?

I am working on obstacle avoidance algorithm. My turtlebot moves according to computed desired V_x and V_y velocities from the algorithm. I will publish these velocities to /Cmd_vel. But I also require to compute yaw. i.e. msg.angular.z.

Later after computing the angle I need to publish to /cmd_vel

What is the math behind computing desired angle based on desired velocities I have? and please also share me in ros code programming python.

Note: The positions will be changed based on dynamic obstacles hence I am not computing the desired angle based on goal position but based on desired velocities. (caz, the algorithm computes velocity obstacles).

edit retag flag offensive close merge delete

Comments

If I am not wrong the maths in each moment should be:

msg.angular.z = msg.angular.z(pre) + atan⁻¹(Vy/Vx).

msg.angular.z should be set originally to your initial orientation (for example 0).

Take into account that in case that Vx is 0 the formula will not work. If Vy has also 0 just keep past orientation, in case Vy has a value consider:

msg.angular.z = msg.angular.z(pre)  +(PI/2)*Sign(Vy)
Solrac3589 gravatar image Solrac3589  ( 2021-03-17 10:15:41 -0500 )edit

Thanks @Solrac, but the problem with this formula is, even if Vx <1 (i.e. 0.1 to 0.9) there is a significant change in theta i.e.( tan-1(Vy/Vx)). This makes the turtle bot rotate in the same place very fast than moving forward. Is there any other way to tackle this situation?

Nagarjun gravatar image Nagarjun  ( 2021-03-17 18:55:00 -0500 )edit
add a comment