NMPC-based Motion Planning with Adaptive Weighting for Dynamic Object Interception

A Cooperative Dual-arm system for dynamic ball catching, coordinated to mimic human upper-body behavior (two arms moving around a shared torso-like reference with shoulder-style synchronization).

Ball Trajectory Interception Point Selection Algorithm

This algorithm selects an optimal interception point from a predicted ball trajectory by filtering points within a safety region, classifying them based on proximity to an object, and choosing the point that requires minimal velocity for interception.The Code is in kalman_filter/kalman_filter_bringup/kalman_filter_action_client_3_time_based.py .

1. Filter Trajectory Points in Safety Region

Input: Full predicted ball trajectory Output: Filtered set of candidate points S.

• Extract only trajectory points that fall within a predefined safety region S.
• This ensures the selected interception point is reachable and safe for the intercepting object.

2. Classify Candidate Points Around the Object

For each point in the filtered set S, calculate its distance to the object center c_obj​ and assign it to one of two sets:

Near-Object Set (S_obj)

• Condition: ∣∣p_curr−c_obj∣∣≤r_obj
• Contains points within the object radius r_obj
• These points are in immediate proximity to the object

Safe-Tracking Set (S_safe​)

• Condition: ∣∣p_curr−c_obj∣∣≤r_safe
• Contains points within the safe tracking radius r_safe
• These points are further away but still trackable

Priority Rule: If a point qualifies for both sets, S_obj takes priority.

3. Evaluate Feasibility and Select Interception Point

Evaluation Order

• First evaluate candidates from S_obj(near-object set)
• If no feasible point found, evaluate candidates from S_safe (safe-tracking set)

Interception Velocity Calculation For each candidate point, compute the required velocity: v_req = ∣∣p_curr−c_obj∣∣/ (t_int − t_min)

Where:

p_curr = current candidate point position

c_obj = object center position

t_int = estimated interception time for this point

t_min​ = minimum time constraint

Key Features and Software Stack

• End-to-end ROS 2 stack: perception → prediction → planning → control (C++/Python)

• AI perception & prediction: YOLO detection + Kalman/EKF for real-time ball trajectory estimation

• Nonlinear MPC planning: CasADi/Ipopt for synchronized dual-arm motion under constraints

• Real-time control: ros2_control torque-PD loop at 1 kHz; ~30 ms planning cycle

• Sim-to-Real robustness: Gazebo/MuJoCo simulation parity and reproducible launch configs

• Safety & limits: joint/velocity/torque limits, timeouts, and recovery behaviors

• Modular design: reusable ROS 2 nodes, clear interfaces , easy extension to new tasks

Video