# Sending turtlesim to Goal Location

When I was studying TF tutorials, I noticed that a calculation is performed in TF listener to send turtle2 to the location of turtle1. It is demonstrated below.

vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));


Is there anybody who could understand this computation ?

I know that it is obvious. Orientation angle and the distance between two turtles are computed. The turtle2 is assumed to be reference point, and pose of turtle1 is computed with respect to frame of turtle2.

What I cannot understand is how this mathematical calculation enables turtle2 to approach turtle1 perfectly. There has to be senseful proof about this. Besides, it is not accurate how the constants 4 and 0.5 are chosen. For example, before seeing this computation, I thought that there has to be a p controller or pid controller code in this TF listener node. However, there is not anything about those controllers. Instead, distance computation exists, and I cannot understand how it really works.

Is there anyone who can explain this ?

edit retag close merge delete

Sort by » oldest newest most voted

The lines of code you refer to act something like a very simple proportional controller. Consider the first line that sets the angular velocity. The transform.getOrigin() call represents a vector from turtle2 to turtle1 expressed in the turtle2 frame. By grabbing just the x and y components of this vector and feeding them into atan2, we are computing a signed angle between the turtle2 x-axis and the vector pointing to turtle1 -- when this angle is zero, turtle2 is pointing straight towards turtle2. We multiply this by a positive gain (randomly chosen to be 4.0) to turn this into a proportional controller for the orientation of turtle2. If we only used this line, this controller would always work to rotate turtle2 to point towards turtle1.

The second line is a proportional controller for the linear velocity of turtle1. Here we compute the distance and then multiply that by an arbitrary gain to set the linear velocity. As turtle2 gets closer to turtle1, the velocity tends to zero.

Note, this system is not really the type of system that you should expect your classic PID-style controller to work well. It's nonlinear, has multiple inputs and outputs, and is nonholonomic (not to mention the fact that the actual implementation of the turtlesim responds to a single command input for a finite amount of time and then automatically zeros the command). The system is often called a differential drive or a unicycle model and there is a ton of research on stable position and trajectory tracking controllers. This simple implementation is just a rough way of getting turtle 2 to follow turtle 1.

more