Uncertainty-Aware Shared Autonomy System with
Hierarchical Conservative Skill Inference

Our system consists of a primary processing unit (MPU) and a robot control unit (RCU), as depicted in Fig. 2. The MPU comprises several components, including VR and camera interfaces, teleoperation, a demonstration repository, skill learning, and real-time data exchange (RTDE). The VR interface captures the user’s motion with synchronized scene images from the camera interface and conveys this data to the teleoperation module. In the teleoperation module, user input motion is converted into robot motion commands, then sent to the RCU via the RTDE. After completing a task demonstration, all teleoperation data, including VR motion, images, and proprioceptive robot states (e.g., joint angles), is stored in the demonstration repository. The skill training module then uses these demonstrations to acquire manipulation skills. The RCU handles communication with the MPU through RTDE, processes user commands, and executes robot control.

In the MPU, the VR interface processes various user commands including controller motion and button events. We conducted teleoperated demonstrations utilizing only the controller without the head-mounted display (HMD), relying on direct observation of Human eyes. As shown in Fig. 2, four dedicated buttons are used for robot teleoperation. The menu button resets the current demonstration episode, the trigger button gives the operator control of the slave robot, the grip button opens and closes the slave robot gripper, and the trackpad button switches the robot’s configuration between forward and downward during teleoperation. Despite HTC VIVE’s left-handed coordinate system, we converted it to a right-handed system to align the local controller coordinate system.

While defining position constraints is straightforward, establishing rotation constraints demands a more intricate approach to ensure precise motion limitations. Specifically, for rotation constraints, we introduced the concept of plane-projected rotational constraints. To implement this, we began by defining projection matrices for three fundamental planes of the $xy$ plane, the $xz$ plane, and the $yz$ plane (Fig. 3):

The formulation of the rotation constraint involves the use of projection matrices for three base planes, including $\hat{P}_{xy}$, which is a $(3\times 2)$ matrix containing the unit column vectors $\hat{\boldsymbol{x}}$ and $\hat{\boldsymbol{y}}$. Similar projection matrices for the $xz$ and $yz$ planes are established in a similar manner.

To implement the rotation constraint, we projected the TCP’s coordinates onto these base planes using the respective projection matrices. This projection allowed us to compute the numerical angles corresponding to roll, pitch, and yaw, effectively constraining rotation. This process exhibits slight variations for each configuration mode, as outlined in equations (6) to (23).

Forward Configuration:

Downward Configuration:

Subsequently, the rotation constraint is finalized by applying rotation clipping thresholds to the actual angle values, which are obtained from the projected TCP coordinates. For instance, in the case of $\beta_{f}$, the process involves projecting the TCP’s $z$-axis direction vector $\boldsymbol{v}^{z}_{\text{tcp}}$ onto the $xz$-plane (Eq. (6)). The angle value is then calculated through the dot-product of the projected vector $\boldsymbol{v}_{xz}$ and the basis vector $\hat{\boldsymbol{x}}$, followed by the arccos function (Eq. (7)). The sign of this angle is set to positive when $v^{z}_{xz}$ is less than zero and negative otherwise by the conditional sign function $\mathcal{C}(x)=\left[(\mathds{1}|x=\text{T})\>\text{or}\>(-\mathds{1}|x=\text{F})\right]$, where $v^{z}_{xz}\in\boldsymbol{v}_{xz}$. Finally, $\beta_{f}$ is confined within the range defined by $\lambda^{\beta_{f}}_{\text{min}}$ and $\lambda^{\beta_{f}}_{\text{max}}$ (Eq. (8)). Additional details regarding the actual threshold values for pose constraints in the forward and downward configurations are presented in Table I.

Human demonstrations for initial skill policy learning were recorded at 30Hz and saved to disk at the end of each episode. Each episode includes TCP trajectories, consisting of observation, robot state, action, and an episode end flag. This data forms the dataset for the target task, denoted as a rollout memory $\mathcal{D}=\{\mathcal{T}_{1},\dots,\mathcal{T}_{N}\}$, where each trajectory $\mathcal{T}_{n}$ comprises packets $\tau_{t}=\{o_{t},;s_{t},;a_{t},u_{t}\}$. A packet includes an RGB image for observation, a 16-dimensional state vector (joint angles, TCP position with quaternion orientation, normalized gripper position, and configuration mode), a 9-dimensional action vector (position, rotation, gripper, and conf. mode), and an episode end flag. The rotation action follows the VR controller’s orientation, with the recorded rotation action determined by the difference between TCP and VR controller orientations:

where the unknown rotation action $\boldsymbol{q}^{\text{act}}_{t}$ is calculated by multiplying the conjugate of the TCP orientation $\boldsymbol{q}^{\text{tcp*}}_{t}$ on both sides of the equation. Algorithm 1 outlines the demonstration dataset collection process.

To learn the manipulation skills from human demonstrations, we designed a hierarchical skill network (HSN) model (Fig. 4) inspired from the SPiRL architecture [11]. HSN consists of a hierarchical structure that infers robot skill embedding from observations of the environment and decodes it into actual robot actions, thereby controlling the real robot. In training phase, HSN learns skill embedding space (pouring and pick and place skills in our case) using recurrent skill encoder $q(z|\mathbb{a_{i}})$ and skill decoder $p_{d}(\mathbb{a_{i}}|z)$ while the skill prior $p_{a}(z_{t}|o_{t},s_{t})$ is trained to learn skill embedding distributions corresponding to the observations by minimizing the Kullback-Leibler divergence [13] between the predicted prior and the inferred skill posterior $\mathbb{E}_{(s,a_{i})\sim\mathcal{D}}D_{KL}(q(z|a_{i}),p_{a}(z|s_{t}))$. To train a robust skill policy, preprocessing steps are applied to the input image, including random cropping, downsizing, and noise addition (Fig. 4). Subsequently, the skill prior generates skill embedding actions from a concatenated vector comprising image features extracted from ResNet18 [14] and the robot state.

In test phase, the skill encoder is not utilized. Instead, the skill prior infers a 12-dimensional skill action $z_{t}=p_{a}(z|o_{t},s_{t})$, which is subsequently decoded by the skill decoder into a H-steps (set to 10) robot action trajectory for application on the physical robot. The decoded robot action ($H$ by 9) encompasses the relative positional difference $\{\partial{x_{a}},\partial{x_{y}},\partial{x_{z}}\}$ and quaternion rotational difference $\{x_{q},x_{z},z_{q},w_{q}\}$ of the end-effector, grip action $g_{a}$ and configuration change action $c_{a}$ that facilitates the alteration of joint configurations between the forward and downward base poses by a button press on the VR controller. As a result, a single skill action inference leads to the execution of a series of actual robot actions spanning $H$-steps as $p_{d}(\mathbb{a_{i}}|z_{t})=\left[a_{t},\dots,a_{t+h},\dots,a_{t+H}\right]$ and $z_{t+1}\cong z_{t+H}$. The skill encoder is composed of a recurrent layer and two linear layers (256-dim, 128-dim), while the skill prior and skill decoder are constructed using three linear layers (256-dim) with leaky-ReLU [15] activation function.

To address the uncertainties in dynamic environments, we applied Monte-Carlo dropout [12] to the skill prior network of HSN. The skill uncertainty associated with the current observation is determined through the standard deviation of the determinants of K covariance matrices:

In our setup (Fig. 2), we used a UR3 robot with a Robotiq 2F-85 gripper for manipulation, an HTC VIVE Pro2 for teleoperation, and a RealSense D435 camera for observation. We adjusted the TCP origin by adding a 127mm offset to its z-axis. Two cameras were placed, one in front for recording evaluation videos and one on the rear side (Fig. 4). Teleoperation and skill learning were conducted on a desktop system with an i7-Xeon processor and an RTX-3090 GPU. For the pick-and-place task, we utilized a white basket with toy fruit, while the pouring task involved a green plastic cup and a transparent bottle containing red beads.

Since the position constraints yielded obvious results, we only conducted an evaluation of the orientation constraints. Fig. 5 illustrates the comparative results between the original and constrained rotation motions of the TCP along each orientation axis. The results demonstrate that the proposed rotation constraint algorithm effectively confines the input rotations to the specified limits as detailed in Table I.

We conducted HSN training over 10K epochs using 308 and 283 demonstration datasets for pouring and pick-and-place tasks. Each demonstration began with randomly positioned target objects within predefined manipulable regions (Fig. 4) for respective tasks. The initial robot configuration was also randomized (forward or downward) with uniform noise. The first half of the dataset was collected via VR teleoperation for initial skill policy training. In the remaining half, correction motion datasets were gathered by user interventions during SAP-based task execution in situations where collisions and task failures (e.g., tipping over a water bottle) were expected. We independently trained pouring and pick-and-place skills and achieved 90% and 80% success rates, respectively, in 10 disturbance-free trials. Failures resulted from minor spatial errors despite correct semantic actions (e.g., approaching the bottle correctly, but failed due to a few spatial errors). This indicates success in semantic learning, with spatial errors expected to decrease with a more extensive and varied dataset. For pick-and-place, occlusion caused by robot hardware made tasks challenging, leading to lower performance compared to pouring, which is relatively simpler. To tackle this, we plan to use multiple cameras for observation in future work.

We assessed our skills in dynamic environments, where target objects were moved during tasks. Despite deliberate disruptions, the agent achieved success rates of 80% and 70% (Fig. 6, Table II). Our HSN demonstrated the ability to adapt to dynamic changes, even without specific disruption demonstrations in the initial dataset. However, adapting to pose variations, like recovering a fallen bottle, would require additional demonstrations.

We evaluated the HSN’s ability to learn multi-configurable skills. Initially, we trained it for 5K epochs using demonstration datasets from two tasks. Then, we collected 200 additional demonstrations with SAP, totaling 10K epochs of training. The results demonstrated the HSN’s success in performing pouring and pick-and-place tasks, achieving 80% and 90% success rates, respectively, over 10 trials. The agent smoothly transitioned configurations between tasks within a few seconds, achieving a 100% success rate in configuration transitions. The outcomes from both single and multi-skill learning, detailed in Table II, suggest the potential extension of this method to a broader range of skills and objects.

The SAP based on Conservative Skill Inference (CSI) demonstrated a more stable learning process compared to existing methods. After collecting a dataset of 70 demonstrations and training for 1K epochs, the non-CSI policy diverged and caused collisions, whereas the CSI-based policy exhibited a more stable motion by reducing the execution speed based on inferred uncertainty (refer to the video). This contributes to the stability of robot motion not only during the learning process but also in out-of-distribution scenarios during deployment.