Motion Execution Algorithm for Smooth Dynamic Replanning in HRC

Motion Execution Algorithm for Smooth Dynamic Replanning in HRC Federico Parma1,2, Cesare Tonola3, Manuel Beschi2,3 Abstract—In human-robot collaboration, robots must operate safely and adapt fluidly within dynamic, shared environments. We introduce THOR (Trajectory receding HOrizon interpolatoR), a model predictive control algorithm designed to explicitly minimize jerk and produce smooth, predictable robot motions. It continuously adapts trajectories in response to dynamic events, like path changes or safety slowdowns, while always respecting joint position, velocity, and acceleration limits. Validated in simulations and real life experiments, THOR significantly reduces jerk and improves motion continuity compared to standard approaches, making it suitable for human-robot interaction scenarios. I. INTRODUCTION In modern industrial applications, robots are increasingly required to operate in dynamic environments and collaborate closely with humans. In such shared workspaces, a robot must be able to react swiftly to changes in its surroundings while adhering to strict safety constraints. The primary goal must be to ensure operator safety, within this design principle a secondary objective is maximizing task efficiency. Standard implementations of the safety ISO/TS 15066 protocols involve reducing the robot speed or executing a complete stop when a human is present, without altering the planned path. While safe, this approach can severely degrade performance. To address this, robots must be capable of realtime path replanning and time parameterization to maintain both safety and efficiency. Existing path-following algorithms, such as Time-Optimal Path Parameterization based on Reachability Analysis (TOPPRA) [1], Iterative Time Parameterization (ITP) [2], and Time-Optimal Trajectory Generation (TOTG) [3], can generate trajectories that respect velocity and acceleration constraints. However, they do not explicitly constrain or minimize jerk, which is a critical parameter in Human-Robot Collaboration (HRC) scenarios. Studies have shown that human operators perceive smoother robot motions with continuous and bounded jerk as more predictable and less risky. Instead, frequent replanning can lead to abrupt changes in the robot’s trajectory, 1Dept. of Electrical and Information Engineering, Polytechnic of Bari, Italy. [email protected] 2Dipartimento di Ingegneria Meccanica e Industriale, University of Brescia, Italy. 3Institute of Intelligent Industrial Technologies and Systems, National Research Council of Italy, STIIMA-CNR, Milan, Italy. Project co-funded by the European Union – Next Generation Eu - under the National Recovery and Resilience Plan (NRRP), Mission 4 Component 2, Investment 3.3 – Decree no. 630 (24th April 2024) of Italian Ministry of University and Research; Concession Decree no. 1956 05th December 2024 adopted by the Italian Ministry of University and Research, CUP D93D24000270003, within the national PhD Programme in Autonomous Systems (XL cycle). compromising motion smoothness and causing uncomfortable or unsafe interactions. This extended abstract introduces THOR (Trajectory receding Horizon interpolatoR), a trajectory execution algorithm designed to address this gap. THOR’s primary contribution is to generate smooth, dynamically feasible trajectories by explicitly minimizing jerk during motion execution, even when the reference path changes frequently. An open-source C++ implementation of the algorithm is publicly available at [4]. II. METHODOLOGY THOR is formulated as a model predictive control algorithm that operates in the robot’s joint space. It builds upon previous work [5] on predictive inverse kinematics but extends the framework to directly optimize joint trajectories and incorporates an explicit jerk minimization term in its cost function. Consider q(t)∈Rnas the joints configuration at time t, where nis the number of degrees of freedom. Then, with pand c, respectively, the number of prediction and control instants, the stacked prediction vector of the d-th derivative is defined as q(d)=q(d)(t0+τ1), ..., q(d)(t0+τp)T∈Rpn. Let u= [u1, . . . , uc]∈Rcn denote the control variable. The control intervals are non-uniform, resulting in sparser control updates as the prediction horizon progresses [5]. The system’s evolution is predicted over a finite horizon using a linear model: q(d)=Ldx0+Fdu,for d∈ {0,1,2,3}. where x0is the initial state of the system, Ld∈Rnp×2n is the free evolution matrix of the d-th derivative, and Fd∈ Rnp×nc is the forced response matrix. A scaling factor s=dξ dt ∈(0,1] is introduced to allow slow downs without altering the geometric path along the nominal trajectory ¯q(ξ(t)) parameterized by the curvilinear abscissa ξ. We define the scaled nominal derivatives as: ¯q(1)(t)=sd¯q(ξ(t)) dξ ,¯q(2)(t)=s2d2¯q(ξ(t)) dξ2. A Sequential Quadratic Programming (SQP) problem is solved over a predictive horizon to find the optimal control inputs and scaling factor. The optimisation problem (1) is a weighed sum of terms designed to minimize tracking errors in position and velocity, while penalizing large acceleration and jerk values and maintaining sclose to unity to avoid unnecessary slowdowns: 2025 I-RIM Conference October 17-19, Rome, Italy ISBN: 9788894580570 10.5281/zenodo.17629716 93 0 5 1010 20 30 40 50 60 70 80 90 100100 0 10 20 30 40 50 60 THOR Frequency (%) ISP 0 5 1010 20 30 40 50 60 70 80 90 100100 0 10 20 30 40 50 60 ITP 0 5 1010 20 30 40 50 60 70 80 90 100100 0 10 20 30 40 50 60 TOTG 0 5 1010 20 30 40 50 60 70 80 90 100100 Joint Jerk (rad/s^3) 0 10 20 30 40 50 60 Spline interpolation Frequency (%) 0 5 1010 20 30 40 50 60 70 80 90 100100 Joint Jerk (rad/s^3) 0 10 20 30 40 50 60 0 5 1010 20 30 40 50 60 70 80 90 100100 Joint Jerk (rad/s^3) 0 10 20 30 40 50 60 Fig. 1. Histogram of the frequency of occurrence of jerk values. min u,s λposwpos +λjerkwjerk +λvelwvel +λaccwacc +λscalingwscaling with wpos =∥q−¯ q∥2 2 wvel =∥q(1) −¯ q(1)∥2 2 wacc =∥u∥2 2 wjerk =  q(3)   2 2 wscaling =∥1−s∥2 2 s.t. qmin ≤q≤qmax q(1) min ≤q(1) ≤q(1) max q(2) min ≤q(2) ≤q(2) max 0< s ≤1 (1) III. EXPERIMENTAL RESULTS THOR was validated through real-world experiments (Fig. 2) and extensive simulations. In simulations, the algorithm was tested on a 6 DoF robot, in a dynamic environment with randomly appearing obstacles, forcing frequent and abrupt path replanning. THOR’s performance was compared against a standard spline interpolator when executing nominal trajectories generated by three different algorithms: Iterative Spline Parametrization (ISP), ITP, and TOTG. Fig. 1shows the frequency distribution of jerk values across specific intervals. The jerk intervals were constructed with a piecewiseuniform spacing, and results demonstrated THOR’s superior performance in maintaining smooth motion: the vast majority of jerk occurrences falls within the [0,1]rad/s3interval. In contrast, the spline interpolator exhibited significantly higher and more widely distributed jerk values. Additionaly, THOR successfully enforced joint velocity and acceleration limits, even when the nominal trajectory (e.g., from ITP) was dynamically unfeasible. On the other hand, spline interpolator sometimes produced shorter task execution times, but in HRC scenarios, the goal is smooth motion for operator comfort and safety, not minimum execution time. The algorithm was deployed on a real collaborative robotic cell featuring a UR10e robot. A human operator intentionally obstructed the robot’s path to trigger real-time replanning. The robot’s motion was monitored using its onboard sensors, and a Fig. 2. Experimental setup in real world experiments. ISO/TS 15066 module was active for safety. The experimental results closely mirrored the simulations: the measured jerk on the robot, showed a similar low-jerk distribution, never exceeding 30rad/s3. A demonstration video is shown in [6]. IV. FUTURE WORKS Future work will focus on incorporating the robot’s dynamics and its proximity to the human operator directly into the control formulation to further enhance safety and perceived trustworthiness. Additionally, forthcoming studies will replace the current static tuning of the SQP cost function weights with a dynamic adaptation rule. The present approach relied on a trial-and-error method, whereas future work will aim to use reinforcement learning techniques to find a dynamic set of optimal parameters. REFERENCES [1] H. Pham and Q. C. Pham, “A new approach to time-optimal path parameterization based on reachability analysis,” IEEE Transactions on Robotics, vol. 34, pp. 645 – 659, 2018. [2] T. Kunz and M. Stilman, “Turning paths into trajectories using parabolic blends,” Georgia Institute of Technology, Tech. Rep., 2011. [3] ——, “Time-optimal trajectory generation for path following with bounded acceleration and velocity,” in Robotics: Science and Systems, 2013, pp. 209–216. [4] https://github.com/JRL-CARI-CNR-UNIBS/thor trajectory lib. [5] M. Faroni, M. Beschi, N. Pedrocchi, and A. Visioli, “Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints,” IEEE Transactions on Robotics, 2019. [6] Video, “Experimantal results,” https://drive.google.com/drive/u/2/folders/ 1NRiRMWj4TRB0vgYnsXJ6Q5US9kBaGvhr. 94