Motion Execution Algorithm for Smooth Dynamic Replanning in HRC

In human-robot collaboration, robots must adapt their motions to dynamic environments while ensuring safety, predictability, and compliance with physical constraints. Frequent trajectory replanning can compromise motion smoothness and lead to unsafe or uncomfortable interactions. This paper introduces THOR (Trajectory receding HOrizon interpolatoR), a model predictive control algorithm in joint space that explicitly minimizes jerk during execution to generate smooth, dynamically feasible trajectories. THOR continuously adapts the trajectory in response to real-time changes, such as path replanning or safety-induced slowdowns, while respecting joint limits on position, velocity and acceleration. THOR is validated through extensive simulation and real-world experiments with a 6-DoF collaborative robotic cell. Results show that THOR significantly reduces jerk and improves motion continuity compared to standard approaches, making it particularly well-suited for responsive and safe behavior in human-robot collaboration scenarios.