Planning & Control

Overview

We design the paddle to shadow the x and y positions of the balloon at a default z height and "pop" to a preset z height when the balloon is expected to reach the paddle's default height.

We keep the paddle tilted toward the center of the drop zone, while holding the paddle at the higher z position for a set number of control ticks.

All target paddle poses are converted into target tool poses and sent to the default MoveIt IK service, which returns target joint states that we convert into velocity commands with a simple custom PID controller.

Balloon Prediction

We subscribe to /ball_poseand maintain a rolling buffer of samples and select the most recent samples to make a simple ballistic model to estimate the ball's position and velocity.

We treat x and y motion as linear, while correcting z motion for gravity. The resulting position and velocity are published as /ball_state for the horizontal tracker.

Paddle Planning

Shadow (x/y component of motion)

Position

We subscribe to /ball_state, which publishes at about 30 Hz, and set the target x and y positions of the paddle to the most recent x and y positions of the ball in the base_link frame.

We orient the paddle so it points away from the robot and toward the center of the drop zone, keeping in mind that the paddle normal is in the tool frame's -x direction.

desired_normal = p_center - desired_p_tool

current_normal = - first vector of quat_to_rot(current_quat)

normalize desired_normal, current_normal

axis = current_normal × desired_normal

angle = arccos(normalized desired_normal · current_normal)

quat from axis, angle

Strike (z component of motion)

We extract the balloon's z velocity and use two conditions to decide when the paddle should pop: whether the balloon is low enough, or whether it is moving fast enough to hit the paddle within a set time window.

When the paddle is popping, it holds the higher z position for a set number of control ticks and then returns to the default z position.