Volumetric Obstacle Avoidance Based on Dynamic Movement Primitives for Robot Path Planning in Human–Robot Collaboration

Abstract

Human–robot collaboration (HRC) can be defined as the close interaction between a human user and a robot working together to accomplish a specific task. True collaboration, however, can only be realized when humans and robots can share the same workspace simultaneously and move freely within it. To address these problems, Learning from Demonstrations (LfD) helps robots become competent in solving plenty of complicated tasks, greatly reducing programming times and allowing task generalization. However, complex robot tasks require complex path planning modeling for a robot to move from one place to another in a heavily constrained workspace following a collision-free path. To this end, a robot programming framework based on Dynamic Movement Primitives (DMPs) is proposed. The framework derives and implements a solution for robot path planning and includes a new DMP formulation with volumetric obstacle avoidance for robot LfD. The formulation equips robotic systems with the capability of online adaptation in the presence of dynamic obstacles. Quantitative evaluations demonstrate high success rates (>96% in tested scenarios) in collision avoidance and typical trajectory adaptation times in the order of milliseconds (<5 ms), supporting its applicability. These methods have been applied in both simulation and real robotic scenarios using a UR10e collaborative robot from Universal Robots for testing and validation purposes. The results indicate that the proposed approach can effectively make the robot follow a user-defined trajectory and learn how to adapt it to avoid collisions with volumetric obstacles of different shapes and poses in an unconstrained human–robot collaborative environment.

1. Introduction

Human–robot collaboration (HRC) can be defined as the close interaction between a human user and a robot working together to accomplish a specific task. True collaboration, however, is only achieved when humans and robots can share the same workspace simultaneously and can perform tasks concurrently during production operation [1]. One of the biggest challenges in collaborative robotics resides in how to achieve full collaboration between humans and robots. Even when collaborative robots (cobots) have gained popularity in industrial settings, it is still uncommon to find them working in close interaction with human operators due to a lack of intuitive robot programming, limiting non-expert operators to create and alter robot programs quickly and intuitively [2]. To overcome these programming complexities and enable robots to learn new skills and adapt to tasks demonstrated by humans within collaborative settings, Learning from Demonstration (LfD) offers a promising paradigm.

LfD aims to equip robots with the ability to acquire new skills by observing movements performed by an expert to solve the same task. This concept is rooted in neuroscience, serving as a means to emulate the human learning process in robots [3]. The LfD process encompasses five stages: Instructor Selection, Data Acquisition, Data Modeling, Task Execution, and Learning Refinement. Initially, the expert who will instruct the task resolution is selected. Typically, the human user who possesses the precise knowledge to solve the task assumes this role. Next, the Data Acquisition phase involves determining how task information will be recorded and how task-specific information will be mapped to the robot’s configuration space. The essence of LfD lies in the Data Modeling process, where a suite of algorithms is employed to learn task features for future reproducibility. Subsequently, during the Task Execution stage, the system’s performance is assessed. Many algorithms utilized in the data modeling phase are designed to be applicable in scenarios not explicitly taught during demonstrations, enabling the evaluation of learning effectiveness. Finally, if necessary, adjustments can be made in the Learning Refinement stage. This stage allows for the fine-tuning of learning parameters or the addition of new information to demonstrations to enhance task execution.

Of course, another major concern is safety. Safety is a crucial aspect of autonomous dynamical systems expected to operate in unknown and unstructured environments. Regarding system properties, safety ensures that undesirable states or events do not occur [4]. Cobots have critical safety requirements due to HRC typically occurring in unstructured and dynamic environments. The coexistence of humans and cobots in shared spaces creates hazardous situations that require appropriate mechanisms to prevent uncontrolled physical contact between them [5]. Avoiding such contact can be viewed as a path-planning problem involving obstacle avoidance. While cobots are designed with inherent safety features, achieving seamless and efficient collaboration in dynamic environments shared with humans remains a significant hurdle. Many existing HRC systems rely on predefined paths or simplistic reactive strategies (e.g., stopping upon collision detection), which can limit fluidity and task efficiency, especially when unexpected obstacles such as moving human co-workers are present. Consequently, there is a pressing need for more advanced techniques that facilitate intuitive robot programming and enable robust, real-time adaptation to dynamic workspaces [2]. Active obstacle avoidance in the LfD framework can enhance cobots’ ability to complete tasks safely and effectively, significantly expanding the LfD application field [6].

This work proposes a LfD programming framework for human–robot collaborative scenarios, designed to enhance collaboration speed and safety by enabling adaptive robot movement in human presence. The framework introduces an improved method of volumetric obstacle avoidance based on Dynamic Movement Primitives (DMPs) formulation [7,8,9], which is a popular LfD algorithm used to represent and generate complex movements, and superquadrics [10], which are a family of geometric shapes used to model a variety of 3D shapes. This enables a robotic manipulator to perform tasks in dynamic, unconstrained environments. The major contribution of this work is the implementation of a complete robotic system for rapid robot programming and task generalization. This includes perception, hand–eye robot coordination, trajectory learning, task adaptation, and obstacle avoidance of objects modeled with superquadric shapes with varying sizes and poses. The robotic system is tested in a collaborative cell, considering human actions alongside robot movements. This contributes to improving the integration of collaborative robots in settings requiring human intervention and action.

The rest of this work is organized as follows: Section 2 presents a literature review on obstacle avoidance in the context of the LfD framework. Then, in Section 3, the basic formulation of DMPs and superquadrics are explained. In Section 4, the proposed programming framework algorithms and implementation steps are thoroughly described. Section 5 is dedicated to validating the experiment design through theoretical and simulation validity before applying the framework to a real human–robot scenario. Section 6 demonstrates the results obtained via experimental evaluation on a real HRC task. Finally, Section 7 discusses the overall performance of the proposed programming framework, its advantages over similar methods, and future research directions and opportunities.

2. Literature Review

In the LfD framework, the learning process occurs during the Data Modelling stage, where the framework gives a way to learn various goal-directed movement skills in high-dimensional continuous state-action spaces directly from human demonstration. As pointed out in [11], there exists a great variety of algorithms focused on LfD. In the context of primitive movement learning, it is possible to classify the learning algorithms into two categories: Deterministic and Probabilistic methods.

The most representative solution of the deterministic methods in LfD is known as DMPs. The term was first introduced by Ijspeert et al. in [7], and further improved in their later work [9]. DMPs have their origin in biological systems, particularly in motor control, and refer to a framework for trajectory learning based on second-order ODEs of the spring-mass-damping type with a forcing term. Furthermore, in the context of the robot obstacle avoidance problem, customized obstacle-avoidance algorithms have been devised utilizing DMPs [12].

While DMP-based methods offer strong generalization capabilities from a few demonstrations, other learning paradigms such as Reinforcement Learning (RL) have also been extensively explored for robot path planning in dynamic environments. For instance, RL enables robots to learn complex behaviors and navigation strategies through interaction and trial-and-error, sometimes even without explicit global information [13]. However, RL approaches often require significant training time and data, and ensuring safety during the learning process in physical HRC scenarios presents a considerable challenge. In contrast, the LfD framework, particularly when augmented with DMPs, aims for faster skill acquisition while providing a structured mathematical basis for incorporating modifications such as the proposed volumetric obstacle avoidance.

To ensure both obstacle avoidance and stability within the DMP framework, a perturbing term is incorporated. This term is typically constructed using potential functions [14], or the scheme of steering angle presented in [15], to facilitate efficient obstacle avoidance while preserving the overall stability of the system. The problem with these solutions is that the coupling term considers point-like obstacles, which are impractical in some situations.

The literature details several attempts to extend the DMP formulation for volumetric obstacle avoidance. One approach, for instance, represents obstacles as point clouds, utilizing the nearest point to the robot in the steering-angle-based obstacle avoidance formulation [16]. Other examples of modeling volumes as point clouds can be found in [17], which addresses issues related to trajectory jittering, ineffective obstacle avoidance in specific scenarios, and better preservation of teaching intentions in dynamic environments for enhanced performance. Meanwhile, ref. [18] employs a method of hyperparameter optimization based on RL to learn both the profiles of potentials and the shape parameters of a motion.

However, as presented by [19], some of the drawbacks of modeling volumes with point clouds are the high computational time due to the density of the point cloud and non-smooth behaviors due to the constantly changing nearest point between the robot and the obstacle. To tackle this, ref. [19] enhanced DMPs to support volumetric obstacle avoidance using superquadric functions in scenarios where obstacles have known and unknown shapes. The superquadrics’ implicit formulation was modified to propose a superquadric potential function based on superellipses that model the shape of the obstacle potential field. This solution was originally developed for static obstacles and was expanded later in [20], showing promising results in dynamic obstacle avoidance in different robotic scenarios. The main drawback of this solution is that convergence and stability of the DMPs plus the potential field is not guaranteed. To avoid these problems, a solution composed of steering angle avoidance and superquadric models was adopted in [21]. The stability and convergence of the method are proven and obstacle avoidance is guaranteed for multiple volumetric obstacles; still, dynamic obstacle avoidance was not tested.

Thus, the proposed solution in this work will extend the work of Ginesi et al. [20] and Liu et al. [21] to:

• Leverage LfD to reduce programming time and enable task generalization for complex robot tasks in constrained workspaces.
• Include superquadrics in the general pose to model static and dynamic obstacles.
• Tackle some of the major drawbacks of the steering angle scheme by addressing limitations like the “dead zone” issue and extending the approach to volumetric obstacles using a modified Mollifier function.
• The framework was successfully tested and validated in both simulations and real-world scenarios using a collaborative robot, demonstrating effective trajectory learning, adaptation, and collision avoidance with static and dynamic obstacles (including human arms) in an HRC setting.

3. Materials and Methods

This section introduces the fundamental concepts underpinning the proposed framework for robot path planning with volumetric obstacle avoidance in HRC. We begin by detailing the standard formulation of DMPs, a prominent technique within LfD for encoding and generalizing trajectories. Following this, we describe superquadrics, the geometric modeling approach used to represent obstacles volumetrically. These core methodologies form the basis for the enhanced DMP formulation and implementation presented in Section 4, which enables reactive collision avoidance in dynamic environments.

3.1. Dynamic Movement Primitives

DMPs aim to generalize a trajectory, even if the initial and final positions change, while maintaining the shape of the original trajectory by modeling its forcing term. In first-order notation, the DMP formulation proposed in [7,9] is:

$$\left\{\begin{array}{c}\tau \dot{\mathbf{v}}=\mathbf{K}({\mathbf{x}}_g-\mathbf{x})-\mathbf{D}\mathbf{v}+({\mathbf{x}}_g-{\mathbf{x}}_0)\mathbf{f}\left(z\right)\hfill \\ \tau \dot{\mathbf{x}}=\mathbf{v}\hfill \end{array}\right.$$

(1)

where $\mathbf{x},\mathbf{v},{\mathbf{x}}_{\mathbf{g}},{\mathbf{x}}_{\mathbf{0}}\in {\mathbb{R}}^n$ are, respectively, the position and velocity of the system, goal and initial positions, and $\mathbf{f}\in {\mathbf{R}}^n$ is the non-linear forcing term. Matrices $\mathbf{K},\mathbf{D}\in {\mathbb{R}}^{n\times n}$ are diagonal matrices representing the elastic and damping terms, usually chosen to make the system critically damped ($\mathbf{D}=2\sqrt{\mathbf{K}}$) for ${\mathbf{x}}_{\mathbf{0}}$ to monotonically converge toward ${\mathbf{x}}_{\mathbf{g}}$. Parameter $\tau \in {\mathbb{R}}^{+}$ is a temporal scaling factor, and parameter $z\in \mathbb{R}$ is a re-parametrization of time, governed by the so-called canonical system. The introduction of this term allows DMPs to synchronize dynamical systems with multiple degrees of freedom, where each degree of freedom can have its equation system but share a common phase.

$$\tau \dot{z}=-\alpha z,\hspace{1em}\alpha >0.$$

(2)

where $\alpha \in {\mathbb{R}}^{+}$ determines the exponential decay.

The forcing term, $\mathbf{f}\left(z\right)={[f_1\left(z\right),f_2\left(z\right),\dots ,f_N\left(z\right)]}^T$, is expressed using a series of basis functions. Each component $f_j\left(z\right)$, $j=1,2,\dots ,N$ is written as

$$f_j\left(z\right)={\displaystyle \frac{{\sum }_{i=1}^N{\omega }_i{\psi }_i\left(z\right)}{{\sum }_{i=1}^N{\psi }_i\left(z\right)}}z$$

(3)

where ${\omega }_i\in \mathbb{R}$ are adjustable weights, and ${\left\{{\psi }_i\left(z\right)\right\}}_{i=1}^N$ is a set of $\mathit{N}$ exponential basis functions. In the literature, Radial Gaussian basis functions are used: given a set of centers ${\left\{c_i\right\}}_{i=1}^N$ and a set of positive widths ${\left\{h_i\right\}}_{i=1}^N$, we have

$${\psi }_i\left(z\right)=exp(-h_i{(z-c_i)}^2).$$

(4)

where $c_i$ are the centers of Gaussian basis functions distributed along the phase of the movement and $h_i$ are their widths. We can define $c_i=exp(-\alpha {\displaystyle \frac{i-1}{N-1}}),h_i={\displaystyle \frac{1}{{(c_{i+1}-c_i)}^2}}$ and $h_N=h_{N-1}$ with $i=1,\dots ,N$.

The learning process consists of the computation of the weights ${\omega }_i\in \mathbb{R}$ that best approximate the desired forcing term ${\mathbf{f}}_d\left(z\right)$, obtained by solving (1) for $\mathbf{f}\left(z\right)$. Given a desired trajectory ${\mathbf{x}}_d\left(t\right)$, $t\in [0,T]$ with velocity $v_d\left(t\right)=\tau {\ddot{\mathbf{x}}}_d\left(t\right)$, this results in:

$${\mathbf{f}}_d\left(z\left(t\right)\right)=\gamma [{\tau }^2{\ddot{\mathbf{x}}}_d\left(t\right)-\tau \mathbf{D}{\dot{\mathbf{x}}}_d\left(t\right)-\mathbf{K}({\mathbf{x}}_g-{\mathbf{x}}_d\left(t\right)]$$

(5)

where $z\left(t\right)=exp(-\alpha t)$, $\gamma ={\displaystyle \frac{1}{{\mathbf{x}}_g-{\mathbf{x}}_0}}$ and ${\mathbf{x}}_g={\mathbf{x}}_d\left(T\right),{\mathbf{x}}_0={\mathbf{x}}_d\left(0\right)$. We can compute the vector $\omega =[{\omega }_1,{\omega }_2,\dots ,{\omega }_N]$ by solving the linear system:

$$\mathbf{\Phi }\omega ={\mathbf{f}}_d$$

(6)

where

$$\mathbf{\Phi }=\left[\begin{array}{ccc}{\displaystyle \frac{{\psi }_1\left(z_0\right)}{{\sum }_{i=1}^N{\psi }_i\left(z_0\right)}}{z}_0& \dots & {\displaystyle \frac{{\psi }_N\left(z_0\right)}{{\sum }_{i=1}^N{\psi }_i\left(z_0\right)}}{z}_0\\ ⋮& \ddots & ⋮\\ {\displaystyle \frac{{\psi }_1\left(z_T\right)}{{\sum }_{i=1}^N{\psi }_i\left(z_T\right)}}{z}_T& \dots & {\displaystyle \frac{{\psi }_N\left(z_T\right)}{{\sum }_{i=1}^N{\psi }_i\left(z_T\right)}}{z}_T\end{array}\right]$$

(7)

3.2. Obstacle Avoidance for DMPs

Another advantage of the DMP formulation is its ability to incorporate additional forcing terms for modeling desired behaviors during trajectory reproduction; for instance, forcing the trajectory to reach specific via points not present during the demonstration. This capability is leveraged in [14] to introduce a coupling term based on potential fields, enabling the implementation of obstacle avoidance within the DMP framework. However, a drawback of potential fields arises when dealing with multiple obstacles, as it can result in the system getting trapped in local minima, and in consequence, no movement can be generated, compromising system stability and convergence. To address such limitations, Hoffmann et al. [15] proposed a formulation based on a steering angle. This formulation was later adopted by [9] in their comprehensive review of DMPs, inspired empirically by human behavior during obstacle avoidance. The original formulation (1) is extended by adding an additional coupling term $\mathbf{p}(\mathbf{x},\mathbf{v})$ to the first differential equation:

$$\tau \dot{\mathbf{v}}=\mathbf{K}({\mathbf{x}}_g-\mathbf{x})-\mathbf{D}\mathbf{v}+({\mathbf{x}}_g-{\mathbf{x}}_0)\mathbf{f}\left(z\right)+\mathbf{p}(\mathbf{x},\mathbf{v})$$

(8)

The differential equation that models human obstacle avoidance behavior is described by the change of the steering angle $\vartheta$ according to

$$\dot{\vartheta }=\gamma \vartheta exp(-\beta |\vartheta \left|\right)$$

(9)

where $\gamma ,\beta \in {\mathbb{R}}_{+}$ are constant gains. As shown in Figure 1 the steering angle can be calculated as

$$\vartheta =arccos\left({\displaystyle \frac{\langle \mathbf{o}-\mathbf{x},\mathbf{v}\rangle }{\parallel \mathbf{o}-\mathbf{x}\parallel \parallel \mathbf{v}\parallel }}\right)$$

(10)

where $\mathbf{o}$ is the position of the obstacle. We can relate the change in the steering angle to the necessary direction change in the velocity vector $\mathbf{v}$ of the robot to avoid the obstacle as

$$\dot{\mathbf{v}}=\mathbf{Rv}\dot{\vartheta }$$

(11)

Matrix $\mathbf{R}$ is a rotational matrix of angle $\pi /2$ with the axis generated by $(\mathbf{o}-\mathbf{x})\times \mathbf{v}$. This velocity change can be used as the coupling term $\mathbf{p}(\mathbf{x},\mathbf{v})$ as

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\gamma \mathbf{Rv}\vartheta exp(-\beta |\vartheta \left|\right)$$

(12)

The issue with the earlier formulation lies in the assumption that each obstacle is treated as a point in space. In practical robotics applications, this presents a challenge because neglecting the shape of obstacles can result in unintended behaviors and collisions. Thus, another approach found in the literature, and upon which the presented solution is based, involves modeling obstacles using superquadric models.

Superquadrics are a family of shapes that includes superellipsoids, supertoroids, and superhyperboloids that can model any shape with a small number of parameters [10]. Superquadrics’ definition is modeled as superellipsoids with varying exponents:

$${\left({\left({\displaystyle \frac{x}{a_1}}\right)}^{{\displaystyle \frac{2}{{ϵ}_2}}}+{\left({\displaystyle \frac{y}{a_2}}\right)}^{{\displaystyle \frac{2}{{ϵ}_2}}}\right)}^{{\displaystyle \frac{{ϵ}_2}{{ϵ}_1}}}+{\left({\displaystyle \frac{z}{a_3}}\right)}^{{\displaystyle \frac{2}{{ϵ}_1}}}=1$$

(13)

where $a_1,a_2$ and $a_3\in {\mathbb{R}}_{+}$ determine the size of the superquadric and ${ϵ}_1$ and ${ϵ}_2\in {\mathbb{R}}_{+}$ decide the shape of it. The explicit representation of it is

$$\mathbf{x}(\eta ,\omega )=\left[\begin{array}{c}{a}_1{cos}^{{ϵ}_1}\left(\eta \right){cos}^{{ϵ}_2}\left(\omega \right)\\ a_2{cos}^{{ϵ}_1}\left(\eta \right){sin}^{{ϵ}_2}\left(\omega \right)\\ a_3{sin}^{{ϵ}_1}\left(\eta \right)\end{array}\right]$$

(14)

where $\eta$ and $\omega$ are bounded $-\pi /2\le \eta \le \pi /2$ and $-\pi \le \omega <\pi$. So, we can define any volumetric shape using only five parameters. In Figure 2, we present some examples of volumetric shapes that can be generated with the abovementioned equation.

4. Implementation

Let us consider (8),

$$\tau \dot{\mathbf{v}}=\mathbf{K}({\mathbf{x}}_g-\mathbf{x})-\mathbf{D}\mathbf{v}+({\mathbf{x}}_g-{\mathbf{x}}_0)\mathbf{f}\left(z\right)+\mathbf{p}(\mathbf{x},\mathbf{v})$$

where $\mathbf{p}(\mathbf{x},\mathbf{v})$ represents a coupling term in the DMP dynamical system formulation. As described in Section 3, the steering angle approaches the coupling term, as described by (12).

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\gamma \mathbf{Rv}\vartheta exp(-\beta |\vartheta \left|\right)$$

This term can be rewritten as

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\gamma \mathbf{Rv}\mathbf{\Theta }$$

(15)

where $\mathbf{\Theta }=\vartheta F_o$ describes the influence of a force generated by an obstacle on the steering angle. This term $F_o$ will be modeled after the general pose description of a superquadric model. Usually, a superquadric centered in a local coordinate system $(x_s,y_s,z_s)$ can be defined by only five parameters ($a_1,a_2,a_3,{ϵ}_1,$ and ${ϵ}_2$). When describing the same superquadric in the general pose, it is necessary to include six additional parameters describing the pose vector relative to the center of the world coordinate system $(x_w,y_w,z_w)$ [22]. Thus, it is required to apply a frame coordinate transformation in the form of

$$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}{x}_s\\ y_s\\ z_s\\ 1\end{array}\right]$$

(16)

where

$$\mathbf{T}=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]$$

(17)

with $r_{ij}$, with $i,j\in {\mathbb{R}}^{+}=\{1,2,3\}$ as the rotation components and $p_x,p_y,\mathrm{and}{p}_z$ as the translation vector. Since it is needed to express this relation in superquadric-centered coordinates, these coordinates are computed as

$$\left[\begin{array}{c}{x}_s\\ y_s\\ z_s\\ 1\end{array}\right]={\mathbf{T}}^{-1}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

(18)

with

$${\mathbf{T}}^{-1}=\left[\begin{array}{cccc}{r}_{11}& r_{21}& r_{31}& p_1\\ r_{12}& r_{22}& r_{32}& p_2\\ r_{13}& r_{23}& r_{33}& p_3\\ 0& 0& 0& 1\end{array}\right]$$

(19)

where $p_1=-(p_x{r}_{11}+p_y{r}_{21}+p_z{r}_{31}),p_2=-(p_x{r}_{12}+p_y{r}_{22}+p_z{r}_{32}),\mathrm{and}{p}_3=-(p_x{r}_{13}+p_y{r}_{23}+p_z{r}_{33})$.

By substituting (18) and (19) into (13), the implicit function representation for superquadrics in the general pose is obtained.

$$\begin{array}{cc}\hfill f_s(x_s,y_s,z_s)=& \\ \hfill & ({\left({\displaystyle \frac{r_{11}{x}_w+r_{21}{y}_w+r_{31}{z}_w+p_1}{a_1}}\right)}^{{\displaystyle \frac{2}{{ϵ}_2}}}\hfill \\ \hfill & +{\left({\displaystyle \frac{r_{12}{x}_w+r_{22}{y}_w+r_{32}{z}_w+p_2}{a_2}}\right)}^{{\displaystyle \frac{2}{{ϵ}_2}}}{)}^{{\displaystyle \frac{{ϵ}_2}{{ϵ}_1}}}\hfill \\ \hfill & +{\left({\displaystyle \frac{r_{13}{x}_w+r_{23}{y}_w+r_{33}{z}_w+p_3}{a_3}}\right)}^{{\displaystyle \frac{2}{{ϵ}_1}}}\hfill \end{array}$$

(20)

Then, considering the work of [21], function $F_o$ can be formulated as

$$F_o={log}_b^{-1}\left(f_s\right)$$

(21)

where $b\in {\mathbb{R}}^{+}$ represents the base of the logarithmic function. As a constant, b determines the magnitude of $F_o$. Unfortunately, dealing with the correct selection of a constant b adds complexity to the formulation. Instead, to reduce the number of extra parameters to tune, Equation (22) is proposed:

$$F_o=exp\left({log}^{-1}\left(f_s\right)\right)$$

(22)

The selection of logarithmic function to describe (22) is simple to understand. When the robot is close to the surface of the obstacle, the function $f_s$ will decrease to 1, and $F_o$ will increase to $F_o\to \infty$, forcing the robot to move away from the obstacle, which is the desired behavior. While the use of the exponential function will preserve its behavior of being strictly increasing and convex, it will grow extremely fast for values of $f_s\to 1^{+}$. So the new DMP coupling term can be expressed as

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\gamma \mathbf{Rv}\vartheta exp\left({log}^{-1}\left(f_s\right)\right)$$

(23)

This term can be further improved following the directions of [16,23]. Both works add an extra term considering the influence of the distance between the robot and the obstacle. If the robot is moving away from the obstacle, the effect of the coupling term should reduce to zero and vice versa; if the robot moves toward the obstacle, the effect of the coupling term should be maximized. Another drawback of the original steering angle formulation is called the “dead zone” in [23]. As the authors described it, the dead zone problem involves a “heading range towards the obstacle for which the system becomes incoherently less reactive." To avoid it, the authors proposed an additional term based on a Gaussian function to solve the dead zone problem. This can be visualized in Figure 3.

Therefore, the coupling term can be enhanced by adding two exponential terms to (15), one $exp(-k{d}^2)$ to regulate the coupling term based on the robot-obstacle distance, and the second, $sign\left(\vartheta \right)exp(-{\vartheta }^2/{\phi }^2)$, to tackle the dead zone problem,

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\gamma \mathbf{Rv}sign\left(\vartheta \right)exp(-k{d}^2)exp\left({\displaystyle \frac{-{\vartheta }^2}{{\phi }^2}}\right)\mathbf{\Theta }$$

(24)

where $\gamma$ and k$\in \mathbb{R}$ are constants. However, this solution is meant to be applied to point obstacles where it is possible to ignore an obstacle when $\vartheta >\pi /2$. When working with volumetric obstacles, angles greater than $\pi /2$ can produce collisions depending on the shape of the obstacle. Considering convex obstacles only, complete obstacle avoidance can be guaranteed only when $\vartheta \ge \pi$ (because the robot will be moving in the opposite direction from the obstacle); it is necessary to modify the Gaussian function to include the interval $\pi /2<\vartheta <\pi$. Nonetheless, the coupling term does not reduce to zero when $\vartheta \to \pi$, as shown in Figure 4a. For this reason, the proposed solution implements a slight modification to the formulation; the Gaussian function is replaced by a Mollifier function obtaining the final form of the proposed coupling term

$$\mathbf{p}(\mathbf{x},\mathbf{v})=\left\{\begin{array}{cc}\gamma \mathbf{Rv}{\vartheta }_{m}exp\left({log}^{-1}\left(f_s\right)\right),\hfill & \mathrm{if}-\phi <\vartheta <\phi \hfill \\ 0,\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(25)

where ${\vartheta }_m=sign\left(\vartheta \right)exp\left({\displaystyle \frac{-1}{1-{\left|\frac{\vartheta }{\phi }\right|}^2}}\right)exp(-k{d}^2)$.

With this modification, the properties of the Gaussian function are preserved and $\mathbf{p}(\mathbf{x},\mathbf{v})\to \mathbf{0}$ when $\vartheta \to \pi$ as demonstrated in Figure 4b.

Algorithm 1 shows how obstacle avoidance is implemented in the proposed solution.

Algorithm 1 Dynamic Movement Primitives with obstacle avoidance

Input:  ${\mathbf{x}}_d$ cartesian trajectory vector, ${\dot{\mathbf{x}}}_d$ velocity vector (optional), ${\ddot{\mathbf{x}}}_d$ acceleration vector (optional), $\mathbf{K}$ stiffness matrix, $\alpha$ canonical system constant, $\tau$ time rescaling factor, T duration of the movement, N number of basis functions, $a_1,a_2,a_3,{ϵ}_1,{ϵ}_2,{\mathbf{p}}_s$ superquadric parameters and pose vector, $\gamma ,k,{\mathbf{v}}_o\mathrm{obstacle}\mathrm{velocity}(\mathrm{optional},\mathrm{if}\mathrm{not}\mathrm{given}\mathbf{0})$  1: $z\leftarrow$ canonical system $exp(-{\displaystyle \frac{\alpha }{\tau }}\ast t)$  2: ${\mathbf{x}}_g\leftarrow {\mathbf{x}}_d\left(T\right)$  3: ${\mathbf{x}}_0\leftarrow {\mathbf{x}}_d\left(0\right)$  4: $z\leftarrow$ canonical system $exp(-{\displaystyle \frac{\alpha }{\tau }}\ast t)$  5: ${\psi }_i\left(z\right)\leftarrow exp(-h_i{(z-c_i)}^2)$ where $c_i=exp(-\alpha {\displaystyle \frac{i-1}{N-1}})$, $h_i={\displaystyle \frac{1}{{(c_{i+1}-c_i)}^2}}$, and $i=1,\dots ,N$  6: $\gamma \leftarrow {\displaystyle \frac{1}{{\mathbf{x}}_g-{\mathbf{x}}_0}}$  7: ${\mathbf{f}}_{\mathbf{d}}\leftarrow$ desired forcing term $\gamma [{\tau }^2{\ddot{\mathbf{x}}}_d-\tau \mathbf{D}{\dot{\mathbf{x}}}_d-\mathbf{K}({\mathbf{x}}_g-{\mathbf{x}}_d)]$  8: $\mathbf{\omega }\leftarrow \mathbf{1}$  9: Initialize $\mathbf{\Phi }$, with ${\displaystyle \frac{{\psi }_i\left(z_j\right)}{{\sum }_{i=1}^N{\psi }_i\left(z_j\right)}}{z}_j$ where $i=1,\dots ,N$ and $j=0,\dots ,T$ 10: Solve $\omega$ do 11:     Locally weighted regression on $\omega ={\mathbf{\Phi }}^{-1}{\mathbf{f}}_d$ 12:     ${\mathbf{P}}_{\mathcal{J}}={\displaystyle \frac{1}{\lambda }}\left({\mathbf{P}}_{\mathcal{J}-1}-{\displaystyle \frac{{\mathbf{P}}_{\mathcal{J}-1}{\mathbf{\phi }}_{\mathcal{J}}{\mathbf{\phi }}_{\mathcal{J}}^{\top }{\mathbf{P}}_{\mathcal{J}-1}}{\lambda +{\mathbf{\phi }}_{\mathcal{J}}^{\top }{\mathbf{P}}_{\mathcal{J}-1}{\mathbf{\phi }}_{\mathcal{J}}}}\right)$ where $\lambda$ is the forgetting factor and ${\mathbf{\phi }}_{\mathcal{J}}$ is the ${}_{\mathcal{J}}\text{-th}$ transposed row of $\Phi$ 13:     ${\mathbf{\omega }}_{\mathcal{J}}={\mathbf{\omega }}_{\mathcal{J}-1}+({\mathbf{f}}_d-{\mathbf{\phi }}_{\mathcal{J}}^{\top }{\mathbf{\omega }}_{\mathcal{J}-1}){\mathbf{P}}_{\mathcal{J}}{\mathbf{\phi }}_{\mathcal{J}}$ 14: end Solve 15: $\mathbf{f}\leftarrow \mathbf{\Phi }\mathbf{\omega }$ 16: $\mathbf{o}\leftarrow p_{sx},p_{sy},p_{sz}$ obstacle position components 17: $\mathbf{v}\leftarrow \dot{\mathbf{x}}-{\mathbf{v}}_{\mathbf{o}}$ 18: ${\mathbf{R}}_o\leftarrow rot(p_{s\phi },p_{s\theta },p_{s\psi })$ obstacle rotation matrix from obstacle orientation components 19: for step $t\in T$ do 20:     $f_s\leftarrow$ (20) with superquadric parameters and ${\mathbf{R}}_o$ 21:     $\mathbf{R}\leftarrow \mathrm{Rot}(\pi /2,(\mathbf{o}-\mathbf{x})\times \mathbf{v})$ 22:     $\vartheta =arccos\left({\displaystyle \frac{\langle \mathbf{o}-\mathbf{x},\mathbf{v}\rangle }{\parallel \mathbf{o}-\mathbf{x}\parallel \parallel \mathbf{v}\parallel }}\right)$ 23:     $d\leftarrow$ distance to obstacle 24:     $\mathbf{p}(\mathbf{x},\mathbf{v})\leftarrow \gamma \mathbf{Rv}{\vartheta }_{m}exp\left({log}^{-1}\left(f_s\right)\right),\mathrm{if}-\phi <\vartheta <\phi \mathrm{else}0$ 25:     $\tau \ddot{\mathbf{x}}\left(t\right)\leftarrow \mathbf{K}({\mathbf{x}}_g-{\mathbf{x}}_d\left(t\right))-\mathbf{D}{\dot{\mathbf{x}}}_d\left(t\right)+({\mathbf{x}}_g-{\mathbf{x}}_0)\mathbf{f}\left(z\right)+\mathbf{p}(\mathbf{x},\mathbf{v})$ 26:     $\tau \mathbf{v}\left(t\right)=\dot{\mathbf{x}}\left(t\right)$ 27:     $\dot{\mathbf{x}}\left(t\right)\leftarrow \dot{\mathbf{x}}\left(t\right)+\tau \ddot{\mathbf{x}}\left(t\right)\ast \mathrm{step}t$ 28:     $\mathbf{x}\left(t\right)\leftarrow \mathbf{x}\left(t\right)+\tau \mathbf{v}\left(t\right)\ast \mathrm{step}t$ 29: end for Output:  Learned $\mathbf{x}$ cartesian trajectory vector, $\dot{\mathbf{x}}$ velocity vector, $\ddot{\mathbf{x}}$ acceleration vector

5. Validation

In this section, the proposed DMP extended formulation will be analyzed from two perspectives: first, mathematical convergence of the system will be proven through formal analysis of Lyapunov stability for dynamical systems, followed by multiple simulation scenarios with multiple obstacles and different trajectories.

5.1. Proof of Stability and Convergence of the System

As it was done in [15], to prove the stability and convergence of the system after the new couple term is added, it is necessary to evaluate the system after a long time has passed. This is $t\to \infty$. As the forcing term depends on z, it decays to 0 exponentially $\mathbf{f}\left(z\right)\to \mathbf{0}$, so (1) can be rewritten as

$$\left\{\begin{array}{c}\dot{\mathbf{v}}=\mathbf{K}({\mathbf{x}}_g-\mathbf{x})-\mathbf{D}\mathbf{v}+\mathbf{p}(\mathbf{x},\mathbf{v})\hfill \\ \dot{\mathbf{x}}=\mathbf{v}\hfill \end{array}\right.$$

(26)

A Lyapunov candidate function $V(\mathbf{x},\mathbf{v})$ is proposed based on the energy function of a damped-spring-mass dynamical system with unitary mass.

$$V(\mathbf{x},\mathbf{v})={\displaystyle \frac{1}{2}}{({\mathbf{x}}_g-\mathbf{x})}^T\mathbf{K}({\mathbf{x}}_g-\mathbf{x})+{\displaystyle \frac{1}{2}}{\dot{\mathbf{v}}}^T\dot{\mathbf{v}}$$

(27)

To guarantee convergence, $\dot{V}<0$ when $\mathbf{v}\ne 0$,

$$\begin{array}{cc}\hfill \dot{V}& ={\nabla }_{\mathbf{x}}{V}^T\dot{\mathbf{x}}+{\nabla }_{\mathbf{v}}{V}^T\dot{\mathbf{v}}\hfill \\ \hfill & =-{({\mathbf{x}}_g-\mathbf{x})}^T\mathbf{K}\dot{\mathbf{x}}+{\mathbf{v}}^T\dot{\mathbf{v}}\hfill \\ \hfill & =-{({\mathbf{x}}_g-\mathbf{x})}^T\mathbf{K}\mathbf{v}+{\mathbf{v}}^T\mathbf{K}({\mathbf{x}}_g-\mathbf{x})-\hfill \\ \hfill & {\dot{\mathbf{x}}}^T\mathbf{D}\dot{\mathbf{x}}+\gamma {\mathbf{v}}^T\mathbf{Rv}\mathbf{\Theta }\hfill \\ \hfill & =-{\mathbf{v}}^T\mathbf{D}\mathbf{v}\hfill \end{array}$$

(28)

The term ${\mathbf{v}}^T\mathbf{Rv}$ is 0, since the rotation matrix $\mathbf{R}$ represents a rotation by $\pi /2$. Thus, ${\mathbf{v}}^T\mathbf{Rv}=|{\mathbf{v}}^T\left|\right|\mathbf{v}|cos\pi /2=0$, also to guarantee damping; the damping matrix $\mathbf{D}$ is chosen to be positive definite. If $\mathbf{v}=0$ and $\mathbf{x}\ne {\mathbf{x}}_g$, then $\dot{V}=0$; however, if $\mathbf{x}\ne {\mathbf{x}}_g$ then $\dot{\mathbf{v}}\ne 0$, and $\dot{V}$ changes. Therefore, from LaSalle’s invariance principle [24], $\mathbf{x}$ converges to ${\mathbf{x}}_g$, and so it determines the proposed solution using the new coupling term.

5.2. Synthetic Experiments

The newly proposed coupling term is evaluated across multiple simulation scenarios. These tests aim to compare the trajectories generated by the formulation in the presence of obstacles, which vary in shape, pose, and velocity. The initial test utilizes a minimum jerk trajectory (MJT) [25] to synthesize an unconstrained point-to-point movement of a human hand. The position of this straight-line segment is defined by:

$$\mathbf{x}={\mathbf{x}}_0+(\mathbf{g}-{\mathbf{x}}_0)(6{\tau }^5-15{\tau }^4+10{\tau }^3)$$

(29)

where $\mathbf{x}$ is the cartesian vector describing the coordinates of the trajectory as a function of time, $\tau$ is the normalized time $t/t_f$ and $0\le \tau \le 1$, and ${\mathbf{x}}_0$ and $\mathbf{g}$ are the initial and final positions of the generated movement.

Subsequently, the MJT trajectory is learned by DMPs with elastic and damping constants $\mathbf{K}=K\mathbf{I}$ and $\mathbf{D}=D\mathbf{I}$, respectively, where $\mathbf{I}$ is the identity matrix and $K=1050$ and $D=2\sqrt{K}$. The hyperparameters for the coupling term are $\gamma =10,k=0.1,\mathrm{and}-\pi \le \phi \le \pi$, and for the validation tests, the obstacle axes are kept symmetric with values of $(0.125,0.125,0.125)$ m, and the center of the obstacles will be fixed in the point $(-0.1,-0.775,0.135)$ m, which is located inside the learned trajectory. The orientation and shape of the obstacles vary to test the validity of the crafted couple-term; for the validation, the $ϵ$ parameters of the superquadrics are in the interval of $[0.1,1.5]$, respectively. Figure 5 illustrates the volumetric obstacle avoidance trajectory. Subsequent figures depicting obstacle avoidance trajectories will be presented in three distinct views: the XY-plane, the XZ-plane, and a 3D Cartesian representation. In these figures, the dashed black line corresponds to the original trajectory learned by the DMPs, while the solid red line represents the recalculated trajectory adjusted for obstacles within the robot’s workspace. The initial and goal positions of the trajectory are indicated by different shape markers: a square denotes the initial position, and a star signifies the goal position.

The rationale for selecting an obstacle whose center point aligns with the learned trajectory is to demonstrate the elimination of the dead-zone issue in the formulation. This is achieved by introducing a mollifier function, which maximizes the impact of the coupling term when the steering angle is zero. It is possible to visualize this effect in Figure 6, which compares the values of the 2-norm speed of the dynamic system represented by the DMPs (hereon, it will be referred to as the robot end-effector), in the presence of obstacles with different geometries, particularly a cube and a sphere. Both speed curves have similar maximum values, but the cube presents a sharper curve; this is correlated to the sharpness of the edges of the geometry.

Additionally, the effectiveness of the proposed coupling term is validated by applying arbitrary rotations to the geometry, demonstrating the extension of the formulation to encompass general pose superquadric functions. Furthermore, the formulation’s robustness is tested against a moving obstacle that follows an MJT for 5 s within the same workspace. Figure 7a,b illustrate flawless obstacle avoidance in both scenarios.

Finally, a more realistic simulated scenario is designed to validate the general applicability of the proposed solution. In this scenario, a trajectory is taught by a human using kinaesthetic teaching, and both single and multiple obstacles are taken into account. Figure 8 displays the trajectories generated for obstacle avoidance in simulations involving single and multiple obstacles. It is evident that the proposed solution exhibits effective obstacle avoidance behavior across various realistic scenarios.

To quantitatively assess the performance of the proposed volumetric obstacle avoidance, all the previously described scenarios were generated randomly across a total of 221 simulated trials. During these tests, the system demonstrated high reliability, achieving a success rate of 97.7% (

Table 1. Success and Failure Rates (Simulated).

Metric	Occurrence (%)
Success Rate	97.74%
Failure Rate	2.26%

). The only failure mode considered was collision, which occurred in 2.3% of the trials. These collision events were typically observed in highly constrained scenarios where the obstacle’s initial position was in immediate proximity to the start or end of the trajectory.

The mean peak positional deviation from the baseline trajectory was 735.55 mm (

Table 2. Descriptive Statistics of Key Performance Metrics for Successful Trials (Simulated).

	Mean	Std. Dev.	Median	Min	Max
Peak Position Error (mm)	735.548	6.609	737.834	723.853	744.996
Tracking Error (RMSE, mm)	14.026	12.996	11.671	1.252	76.306
Max Computation Time (ms)	2.507	0.795	2.344	1.323	7.631
99th Percentile Latency (ms)	1.882	0.626	1.862	1.171	3.358
Computational Overhead (%)	97.890	0.398	98.147	97.105	98.378

) and 740.08 mm (

Table 3. Descriptive Statistics of Key Performance Metrics for Failed Trials (Simulated).

	Mean	Std. Dev.	Median	Min	Max
Peak Position Error (mm)	740.080	9.192	744.996	723.853	744.996
Tracking Error (RMSE, mm)	2.748	1.588	2.357	1.156	5.422
Max Computation Time (ms)	3.011	0.562	3.210	2.340	3.707
99th percentile latency (ms)	2.191	0.549	1.927	1.884	3.163
Computational Overhead (%)	98.435	0.389	98.598	97.742	98.648

) for the successful and failed trials, respectively. The reactive planner maintained high computational efficiency, with a mean 99th percentile latency computation time of 1.88 ms (Table 2).

6. Results

The proposed solution was tested on a UR10e collaborative robot from Universal Robots (Odense, Denmark). The robot has six DoF and 1300 mm of reach, capable of lifting 12.5 kg and reaching speeds up to 3.142 rad/s in its joint configuration. The robot is equipped with a wrist camera and a two-finger gripper model 2F85 of the brand Robotiq (Levis, QB, Canada), as shown in Figure 9.

Likewise, is important to select the robot’s external sensors for collecting environmental data. As volumetric information about the environment is necessary, the ZED 2 stereo camera from StereoLabs (San Francisco, CA, USA) is selected. The ZED 2 stereo camera has the advantage of capturing at 60 fps in HD resolution or 30 fps in full-HD resolution, and additionally comes with some built-in object and human tracking features through its SDK, which are useful for deployment.

The workstation used for the implementation possesses an AMD Ryzen 5600G processor capable of running up to 4.4 GHz of clock frequency, and also has 32 GB of RAM and an RTX3060 NVIDIA GPU with 12 GB of DDR6 dedicated memory as its main components.

The experiments will utilize the same basic experimental setup, centered on the UR10e manipulator as shown in Figure 10. The task involves a pick-and-place operation where the robot will move a red cube of 2.5 cm from an arbitrary point in its workspace to a short pipe segment of 7.5 cm diameter, also arbitrarily positioned within the workspace. This operation can be described by a simple state machine with five actions/states: (1) visually locating the object to grasp and the target placement position; (2) moving to the object; (3) grasping it; (4) moving to the target position; and (5) releasing the object. The critical part for evaluating the algorithm’s effectiveness is the movement performed after grasping the object of interest until reaching the target position. Key performance metrics for the algorithm will include the computation time for each robot movement step and the position error relative to the calculated DMP trajectory in an obstacle-free environment.

For testing purposes, we prepared five distinct trajectories to simulate unconstrained point-to-point human hand movements. One trajectory was modeled using a MJT to represent straight-line segments. Two trajectories were directly recorded from a human user’s right-hand movements via the ZED 2 camera. Each recorded trajectory was then smoothed by fitting a five-degree polynomial (e.g., Figure 11). The remaining two trajectories were derived by flattening the recorded and smoothed trajectories.

The Cartesian vector describing these trajectories is defined in the global space frame, with its origin at the robot’s base link. Here, ${\mathbf{x}}_0$ and $\mathbf{g}$ denote the initial and goal positions for the pick-and-place operation. These positions are determined by localizing the object to be grasped (a small red cube) and the target position within the pipe segment, as illustrated in Figure 12. Cases where obstacles obstruct either the initial or goal positions during execution are excluded from this study. Such scenarios consistently yield erroneous results and pose potential risks to both the user and the robot, thus falling outside the scope of this research. Each experiment utilizes one of these five distinct trajectory shapes.

For each experiment, the trajectory is learned by a DMP with $N=100$ as the number of basis functions, and elastic and damping constants $\mathbf{K}=K\mathbf{I}$ and $\mathbf{D}=D\mathbf{I}$, respectively, where $\mathbf{I}$ is the identity matrix and $K=3550$ and $D=2\sqrt{K}$. The hyperparameters for the coupling term are $\gamma =10,k=0.1,\mathrm{and}-\pi \le \phi \le \pi$. To describe the volumetric shapes of the obstacles, the $ϵ$ parameters of the superquadrics are in the interval of $[0.1,1.0]$, and the axes will be determined by the dimensions of the bounding boxes around the detected objects. The work table of the robot is described by a superquadric shape with $ϵ=[0.1,0.1]$ parameters, volume axes $[0.97,0.5,0.05]$, and a volume center in $[0,-0.615,-0.010]$.

The framework’s performance was validated across 77 real-world trials, achieving an overall success rate of 96.1% (

Table 4. Success and Failure Rates (Real).

Category	Breakdown	Occurrence (%)
Failure Rate	Acceleration Limit	2.60%
	Collision	1.30%
	Total	3.90%
Success Rate		96.10%

). Failures (3.9%) were categorized into two modes: direct collisions were detected in 1.3% of trials, and precautionary stops due to exceeding the acceleration safety limit occurred in 2.6% of scenarios. The latter represents cases where the algorithm computed a valid, aggressive maneuver that was intentionally halted to ensure the physical integrity of the operator and the robotic system.

On successful runs, the mean peak deviation was 167.32 mm, and the mean 99th percentile latency computation time was 2.939 ms per step (

Table 5. Descriptive Statistics of Key Performance Metrics for Successful Trials (Real).

	Mean	Std. Dev.	Median	Min	Max
Peak Position Error (mm)	167.324	53.403	178.951	1.117	276.873
Tracking Error (RMSE, mm)	8.180	2.460	8.222	3.511	20.251
Max Computation Time (ms)	3.861	0.264	3.856	3.549	5.509
99th Percentile Latency (ms)	2.939	0.123	2.897	2.742	3.175
Computational Overhead (%)	48.237	0.251	48.281	47.645	48.829

), confirming real-time feasibility. The worst-case computation time observed across all trials was 5.51 ms, indicating robust and predictable performance under the tested conditions.

Conversely, performance metrics for unsuccessful trials, including a mean 99th percentile latency of 3.022 ms, are detailed in

Table 6. Descriptive Statistics of Key Performance Metrics for Failed Trials (Real).

	Mean	Std. Dev.	Median	Min	Max
Peak Position Error (mm)	123.202	206.338	6.479	1.683	361.444
Tracking Error (RMSE, mm)	6.810	7.069	3.043	2.422	14.965
Max Computation Time (ms)	3.676	0.579	3.575	3.153	4.299
99th Percentile Latency (ms)	3.022	0.242	2.921	2.846	3.299
Computational Overhead (%)	40.021	5.180	40.023	34.840	45.200

for comparison. These results suggest that failures were likely due to physical constraints or trajectory issues rather than computational bottlenecks.

Furthermore, in experiments involving humans, only the right arm of the user will be visually tracked. This restriction is implemented to streamline computational processes and enable the left arm of the user to adjust the goal position dynamically during execution without compromising system performance. Additionally, the cobot maximum speed was limited to 250 mm/s, and precautionary stops due to exceeding the acceleration safety limit were kept in place, as described previously.

In these experiments, the human right arm is treated as an obstacle, and the corresponding obstacle avoidance trajectories are depicted in Figure 13. Figure 13a–c illustrate different scenarios where the right arm of the human is represented using various superquadric shapes. Different colors on the geometries indicate the initial and final positions of the right arm tracked by the ZED 2 camera: purple denotes the initial position and green denotes the final position, respectively.

As previously mentioned, the user has the capability to adjust the goal position of the trajectory during execution to demonstrate the system’s ability to converge to this new goal position. In each trajectory, a star marker indicates how the goal target position is modified during task execution, while the movements of the human right arm are represented by solid black lines.

Figure 13d illustrates the execution timesteps of the pick-and-place operation, demonstrating successful obstacle avoidance and online adaptation within the shared workspace in the presence of a human.

7. Discussion & Conclusions

This work presents a robot programming framework designed for robot LfD within human–robot collaborative environments. The framework has been developed, implemented, validated, and tested both in simulated environments and on a physical robotic station. It incorporates an enhanced formulation of DMPs that includes volumetric obstacle avoidance capabilities in Cartesian space.

The formulation implemented not only considers the geometric shape of obstacles but also their relative position, orientation, and velocity in relation to the robot. This advancement extends traditional obstacle avoidance techniques from point obstacles to volumetric obstacles, applicable to various shapes using superquadric functions as its fundamental approach. A key advantage of this method is its ability to maintain the stability and convergence properties of the original DMP formulation, even in scenarios involving dynamic obstacles.

The physical robotic station consists of an industrial collaborative manipulator equipped with a visual perception system for object and human detection and tracking. The volumetric obstacle avoidance was tested on a human–robot environment, where the robot must complete a pick-and-place operation while the user moves around in the shared workspace, and therefore the solution gives insights into human–robot interaction (HRI) in HRC task scenarios.

The quantitative results presented (Table 2 and Table 5) underscore the effectiveness of our proposed formulation. In contrast to traditional DMP obstacle avoidance techniques that often simplify obstacles to points or use potential fields prone to local minima with multiple obstacles, our approach directly models and reacts to volumetric obstacles of varying shapes and poses using superquadrics in a general pose. The demonstrated success in avoiding dynamic obstacles, including human limbs in HRC scenarios (Table 4), with an average computation time of 2.939 ms per adaptation step, highlights its practical applicability. Unlike point-cloud-based avoidance methods, which can be computationally demanding due to high data density and may result in non-smooth trajectories from rapidly changing nearest-point calculations, our superquadric modeling offers a compact and efficient representation that facilitates smoother and more predictable avoidance maneuvers.

The main drawback of the proposed method is the number of necessary hyperparameters for tuning, an activity that is critical depending on the volume shape used for modeling the obstacles. For example, the proposed coupling term is primarily influenced by the parameter $\gamma$, which significantly affects movement generation. Varying $\gamma$ results in different performances in obstacle avoidance, and this behavior was qualitatively observed during the tuning of the algorithm for the experiments. Specifically, a smaller $\gamma$ may cause movement failure in obstacle-avoidance scenarios, while a larger $\gamma$ can lead to poor performance in tracking the desired trajectory. This can result in unrealistic acceleration values that are either unattainable physically or compromise the safety of the robot and its user. This behavior could be minimized with the correct selection of the hyperparameters, but still needs to be proven.

To address this, future research will explore options for automated hyperparameter optimization. Techniques such as RL, Bayesian optimization, or evolutionary algorithms could be investigated to systematically tune these parameters, potentially enhancing performance consistency across different tasks and reducing the manual setup effort.

Another limitation is the orientation of the robot’s end-effector, which has not been taken into account during the robot learning phase. In both path planning methods, the robot end-effector is considered a rigid point-like robot, and its volume and dimensions have been omitted during the experiments. Rather than maintaining a static end-effector configuration, the orientation should be adaptively adjusted in real-time, taking into consideration factors such as the demonstrated trajectory, the type and position of obstacles, and the object being grasped. To address this limitation, options for integrating the volumetric obstacle avoidance in quaternion DMP formulation to include the end-effector movements during the trajectory adaptation will be explored.

It is also important to acknowledge that while superquadrics provide a versatile and computationally efficient method for modeling many common object shapes, representing highly complex, non-convex, or finely articulated geometries, such as the full human body, with a single superquadric might have limitations in terms of geometric fidelity. Future investigations could explore two possible solutions. First, utilizing multiple superquadrics to form more intricate composite object representations could effectively model non-convex geometries, allowing the algorithm to treat these obstacles as chains of simpler superquadric shapes, thereby improving avoidance success rates. Second, hybrid approaches that combine superquadrics for intricate shapes with other techniques (e.g., point clouds or mesh segments) could be employed to capture local detail where high precision is crucial.

Future work will also focus on validating the system’s robustness against a wider array of dynamic obstacle behaviors. This includes testing with non-linear obstacle trajectories, variable and unpredictable speeds, and more complex articulated movements to further assess the algorithm’s performance limits and adaptability in highly dynamic and unstructured HRC environments.

While the presented real-time adaptation times are promising, a complete assessment of safety in HRC requires a detailed evaluation of emergency stop response time and adherence to minimum safety distances. Our measured 99th percentile latency computation time of 2.939 ms per step for real successful trials provides a crucial component of the overall response. However, the total emergency stop response time also incorporates factors such as sensor refresh rates, communication latency within the robot control architecture, and the physical braking capabilities of the UR10e manipulator. Future work aims to quantify this end-to-end response time through dedicated experiments, focusing on the total duration from the detection of an unsafe condition (for example, human intrusion into a predefined safety zone) to the robot achieving a complete halt or a verified safe state. Furthermore, while our system exhibited a low collision rate of 1.3% in real-world trials and utilizes ’precautionary stops’ when acceleration limits are exceeded, explicit definition and verification of the maintained minimum safety distance is essential. This will involve establishing quantifiable safety envelopes based on relevant safety standards (ISO 10218 [26], ISO/TS 15066 [1]) and employing precise tracking methods to confirm that the robot’s adapted trajectory consistently respects these distances, even under aggressive maneuvers. The influence of the $\gamma$ parameter in the coupling term on implicitly maintaining these safety distances will also be a subject of further investigation.

Regardless of these limitations and future improvements, the initial objective of developing an LfD programming framework on a collaborative robot to improve HRC in a manufacturing task involving reactive path planning and volumetric obstacle avoidance has been accomplished. The findings of this study will help lay the foundation for further enhancements in the LfD model and better interactions between humans and robots in unconstrained environments.