Given a stream of 3D ball coordinates from vision, we want to predict the future positions of the ball, and then determine a good target point to move the end effector of the arm to strike the ball. Finally, we publish the point to a topic for access by the controller.
Given the stream of 3D ball coordinates in the robot frame from vision as input, we define 2 depth planes th1 and th2, with th1 > th2 meaning that the th1 plane is farther away from the robot. When the system picks up the first ball coordinate that crosses the th1 plane with depth value between th1 and th2, we use this coordinate as the first point \(x\). We then use the first ball coordinate that crosses the th2 plane with depth value less than th2 as the second point \(x'\). We compute the velocity as \((x' - x) / (t' - t)\), where \(t\) and \(t'\) are obtained from system time using Python's time library. This setup offers a robust way to detect ball launch as the ball can be freely moved at any velocity and not count as a ball launch as long as it's not crossing the depth planes.
Given the initial velocity, we use a simple projectile motion calculation to obtain predicted future positions of the ball. We only account for gravity, which works well enough for our lab setting.
Horizontal motion (x):
\[ x(t) = x_0 + v_{0x} t \]
Vertical motion (y):
\[ y(t) = y_0 + v_{0y} t - \frac{1}{2} g t^2 \]
Depth motion (z):
\[ z(t) = z_0 + v_{0z} t \]
Where:
To pick a strike point while ensuring a safe range of operation of the Sawyer due to the fast movements, we define a strike zone around the Sawyer robot and select the first point in the predicted trajectory that enters the strike zone as the strike point. This setup ensures that the target position is always within predefined limits. For ball throws that are too far away from the robot or for occasional errors in predicted trajectory such that the predicted trajectory doesn't intercept the strike zone, then no strike point would be published.
