Model Predictive Manipulation of Compliant Objects with Multi-Objective Optimizer and Adversarial Network for Occlusion Compensation

Many researchers have previously studied this challenging problem (we refer the reader to [5, 6] for comprehensive reviews). To servo-control the object’s non-rigid shape, it is essential to design a low-dimensional feature representation that can capture the key geometric properties of the object. Several representation methods have been proposed before, e.g., geometric features based on points, angles, curvatures, etc [7, 8]; However, due to their hard-coded nature, these methods can only be used to represent a single shaping action. Other geometric features computed from contours and centerlines [9, 10, 11] can represent soft object deformation in a more general way. Various data-driven approaches have also been proposed to represent shapes, e.g., using fast point feature histograms [12], bottleneck layers [13, 14], principal component analysis [15], etc. However, there is no widely accepted approach to compute efficient/compact feature representations for 3D shape; This is still an open research problem.

Occlusions of a camera’s field of view pose many complications to the implementation of visual servoing controllers, as the computation of (standard) feedback features requires complete visual observations of the object at all times. Many methods have been developed to tackle this critical issue, e.g. [16] used the estimated interaction matrix to handle information loss in visual servoing, yet, this method requires a calibrated visual-motor model. Coherent point drift was utilized in [17] to register the topological structure from previous sequences and to predict occlusions; However, this method is sensitive to the initial point set, further affects the registration result. A structure preserved registration method was presented in [18] to track occluded objects; This approach has good accuracy and robustness to noise, yet, its efficiency decreases with the number of points. To efficiently implement vision-based strategies in the field, it is essential to develop algorithms that can robustly guide the servoing task, even in the presence of occlusions.

To visually guide the manipulation task, control methods must have some form of model that (at least approximately) describes that how the input robot motions produce output shape changes [19]; In the visual servoing community, such differential relation is typically captured by the so-called interaction (Jacobian) matrix [20]. Many methods have been proposed to address this issue, e.g. the Broyden update rule [21] is a classical algorithm to iteratively estimate this transformation matrix [7, 22, 23]. Although these types of algorithms do not require knowledge of the model’s structure, its estimation properties are only valid locally. Other approaches with global estimation properties include algorithms based on (deep) artificial neural networks [2], optimization-based algorithms [24], adaptive estimators [25], etc. However, the majority of existing methods estimate this model based on a single performance criterion (typically, a first-order Jacobian-like relation), which limits the types of dynamic responses that the robot can achieve during a task. A multi-objective model estimation is particularly important in the manipulation of compliant materials, as their mechanical properties are rarely known in practice, thus, making hard to meet various performance requirements.

To compute the active shaping motions, most control methods only formulate the problem in terms of the final target shape and do not typically consider the system’s physical constraints. MPC represents a feasible solution to these issues, as it performs the control tasks by optimizing cost functions over a finite time-horizon rather than finding the exact analytical solution [26]; This allows MPC to compute controls that guide the task while satisfying a set of constraints, e.g., control saturation, workspace bounds, etc. Despite its valuable and flexible properties, MPC has not been sufficiently studied in the context of shape control of deformable objects.

To solve the above-mentioned issues, in this paper we propose a new control framework to manipulate purely-elastic objects into desired configurations. The original features of our new methodology include: 1) A parametric shape descriptor to efficiently characterize 3D deformations based on online curve/surface fitting; 2) A robust shape prediction network based on adversarial neural networks to compensate visual occlusions; 3) An optimization-based estimator to approximate the deformation Jacobian matrix and satisfy various performance constraints; 4) An MPC-based motion controller to guide the shaping motions while simultaneously solving workspace and saturation constraints.

To the best of the authors’ knowledge, this is the first time that a shape servoing controller is developed with all the functions proposed in this paper. To validate the effectiveness of our new methodology, we report a detail experimental study with a robotic platform manipulating various types of compliant objects.

Let us denote the position of the robot’s end-effector by $\mathbf{r}\in\mathbb{R}^{3}$. As the dimension of the $3N$ observed points $\bar{\mathbf{c}}$ is typically very large, therefore, its direct use as a feedback signal for shape servocontrol is impractical. Therefore, an efficient controller may typically require to use some form of dimension reduction technique. To deal with this issue, we construct a feature vector $\mathbf{s}=\mathbf{f}_{s}(\bar{\mathbf{c}}):\mathbb{R}^{3N}\mapsto\mathbb{R}^{p}$, for $p\ll 3N$, to represent the geometric feedback $\bar{\mathbf{c}}$ of the object. This compact feedback-like signal will be used to design our automatic 3D shape controller.

For purely elastic objects undergoing deformations, it is reasonable to model that the object configuration is only dependant on its potential energy $\mathcal{P}$ (thus, all inertial and viscous effects are neglected from our analysis [10]). We model that $\mathcal{P}$ is fully determined by the feedback feature vector $\mathbf{s}$ and the robot’s position $\mathbf{r}$ [27], i.e.: $\mathcal{P}=\mathcal{P}(\mathbf{s},\mathbf{r})$. In steady-state, the extremum expression satisfies $(\partial\mathcal{P}/\partial\mathbf{s})^{\intercal}=\mathbf{a}(\mathbf{s},\mathbf{r})=\mathbf{0}$ [28]. We then define the following matrices:

which are useful to linearize the extremum equation (by using first-order Taylor’s series expansion) as follows:

for $\Delta\mathbf{s}$ and $\Delta\mathbf{r}$ as small changes. Note that as $\mathbf{a}(\mathbf{s}+\Delta\mathbf{s},\mathbf{r}+\Delta\mathbf{r})=\mathbf{a}(\mathbf{s},\mathbf{r})=\mathbf{0}$ is satisfied, we can obtain the following motion model:

where $\mathbf{J}(\mathbf{s},\mathbf{r})=-\mathbf{G}(\mathbf{s},\mathbf{r})^{-1}\mathbf{K}(\mathbf{s},\mathbf{r})\in\mathbb{R}^{p\times 3}$ represents the deformation Jacobian matrix (which depends on both the feature vector and robot position), and $\mathbf{u}$ represents the robot’s motion control input. This model can be expressed in an intuitive discrete-time form:

The deformation Jacobian matrix (DJM) $\mathbf{J}_{k}$ indicates how the robot’s action $\mathbf{u}_{k}=\mathbf{r}_{k}-\mathbf{r}_{k-1}$ produces changes in the feedback features $\Delta\mathbf{s}_{k+1}=\mathbf{s}_{k+1}-\mathbf{s}_{k}$. Clearly, the analytical computation of $\mathbf{J}_{k}$ requires knowledge of the physical properties and model of the elastic object and the vision system, which are difficult to obtain in practice. Thus, numerical methods are often used to approximate this matrix in real-time, which enables to perform vision-guided manipulation tasks.

In this paper, we consider a robot manipulator whose control inputs represent velocity commands (here, modelled as the differential changes $\Delta\mathbf{r}$). It is assumed that $\Delta\mathbf{r}$ can be instantaneously executed without delay [11].

Problem statement. Design a vision-based control method to automatically manipulate a compliant object into a target 3D configuration, while simultaneously compensating for visual occlusions of the camera and estimating the deformation Jacobian matrix of the object-robot system.

Although LSM has an efficient one-step calculation, the weights of $B_{j,n}$ are the same all over the variable parameters (e.g., $\rho$ or $x,y$). Thus, to approximate complex curves/surfaces, the fitting order needs to be increased, which may lead to over-fitting problems. To address this issue, MLS assumes that $\mathbf{s}$ is parameter-dependent, i.e., it changes with respect to $\rho$ (for centerline and contour), or to $(x,y)$ (for surface). This enables MLS to represent complex shapes with a lower fitting order. A support field [36] is introduced to ensure that the value of each weight is only influenced by data points within the support field. A conceptual diagram is given in Fig. 3.

As the input data $\bar{\mathbf{c}}_{k},\mathbf{r}_{k},\mathbf{u}_{k}$ to the network have different sizes, they need to be rearranged into structures with unified dimensions. To this end, $\bar{\mathbf{c}}_{k}$ is first rearranged into the matrix $\mathbf{T}^{high}_{k}=[\mathbf{c}_{1},\mathbf{c}_{2},\ldots,\mathbf{c}_{N}]^{\intercal}\in\mathbb{R}^{N\times 3}$. Then, farthest point sampling (FPS) [41] is used to downsample $\mathbf{T}^{high}_{k}$ to two resolutions $\mathbf{T}^{mid}_{k}\in\mathbb{R}^{\frac{N}{\delta}\times 3}$ and $\mathbf{T}^{low}_{k}\in\mathbb{R}^{\frac{N}{\delta^{2}}\times 3}$, for $\delta$ as the resolution scale. Finally, the vectors $\mathbf{r}_{k}$ and $\mathbf{u}_{k}$ are rearranged into the matrices $\boldsymbol{\Lambda}_{k}^{high}={\mathbf{I}_{N\times 1}}\otimes\mathbf{r}_{k}^{\intercal}\in\mathbb{R}^{N\times 3}$ and $\boldsymbol{\Sigma}_{k}^{high}={\mathbf{I}_{N\times 1}}\otimes\mathbf{u}_{k}^{\intercal}\in\mathbb{R}^{N\times 3}$, which are similarly downsampled into mid and low resolutions as follows:

Thus, three different resolutions are generated for the network, high $\{\mathbf{T}_{k}^{high},\boldsymbol{\Lambda}_{k}^{high},\boldsymbol{\Sigma}_{k}^{high}\}$, mid $\{\mathbf{T}_{k}^{mid},\boldsymbol{\Lambda}_{k}^{mid},\boldsymbol{\Sigma}_{k}^{mid}\}$, and low $\{\mathbf{T}_{k}^{low},\boldsymbol{\Lambda}_{k}^{low},\boldsymbol{\Sigma}_{k}^{low}\}$, that provides a total input data of dimension $[N\times 9,\frac{N}{\delta}\times 9,\frac{N}{\delta^{2}}\times 9]$. $\mathbf{T}_{k}^{high},\mathbf{T}_{k}^{mid},\mathbf{T}_{k}^{low}$ are the geometric shapes of the object under different compression sizes specified by $\delta$. Thus, these three terms can describe the potential feature structure of objects. The proposed SPN aims to predict the object’s shape that results from the robots actions, under the current object-robot configuration. By using this multi-resolution data {high, mid, low}, the encoder can better learn the potential feature information of shapes.

A combined multi-layer perceptron (CMLP) is the feature extractor of MRE, which uses the output of each layer in a MLP to form a multiple-dimensional feature vector. Traditional methods adopt the last layer of the MLP output as features, and do not consider the output of the intermediate layers, which leads to potentially losing important local information [42]. CMLP enables to make good use of low-level and mid-level features that include useful intermediate-transition information [40]. CMLP utilizes MLP to encode input data into multiple dimensions $[64,128,256,512,1024]$. Then, we maxpool the output of the last four layers to construct a multiple-dimensional feature vector as follows:

The combined feature vector is constructed as: $\bar{\mathbf{v}}_{i}=[\mathbf{v}_{1}^{\intercal},\mathbf{v}_{2}^{\intercal},\mathbf{v}_{3}^{\intercal},\mathbf{v}_{4}^{\intercal}]^{\intercal}\in\mathbb{R}^{1920}$. Three independent CMLPs map three resolutions into three individual $\bar{\mathbf{v}}_{i},$ for $i=1,2,3$. Each $\bar{\mathbf{v}}_{i}$ represents the extracted potential information of each resolution. Then, the augmented feature matrix $\mathbf{V}$ is generated by arranging its columns as $\mathbf{V}=[\bar{\mathbf{v}}_{1},\bar{\mathbf{v}}_{2},\bar{\mathbf{v}}_{3}]\in\mathbb{R}^{1920\times 3}$, and further through 1D-convolution to obtain $\mathbf{M}\in\mathbb{R}^{N\times 3}$. Finally, the predicted next-moment shape $\hat{\bar{\mathbf{c}}}_{k+1}\in\mathbb{R}^{3N}$ is obtained by vectorizing $\mathbf{M}$. The prediction loss of MRE is:

where $\bar{\mathbf{c}}_{k+1}$ represents the ground-truth next-moment shape in the training data-set.

Generative Adversarial Network (GAN) is chosen as DN to enhance the prediction accuracy. For simplicity, we define $\Phi=\rm{MRE}()$ and $\Psi=\rm{DN}()$. We define ($\bar{\mathbf{c}}_{k},\mathbf{r}_{k},\mathbf{u}_{k}$) as the $\mathcal{X}$ input into $\Phi$, while $\mathcal{Y}$ represents the true shape $\bar{\mathbf{c}}_{k+1}$. $\Psi$ is a classification network with similar structure as CMLP, constituted by serial MLP layers $[128,256,512,1024]$ to distinguish the predicted shape $\Phi(\mathcal{X})$ and the real shape $\mathcal{Y}$. We maxpool the last three layers of $\Psi$ to obtain feature vector $[256,512,1024]$. Three feature vectors are concatenated into a latent vector $\mathbf{m}\in\mathbb{R}^{1792}$, and then passed through the fully-connected layers $[512,256,64,1]$ followed by sigmoid-classifier to obtain the evaluation. The adversarial loss is defined as follows:

where $\alpha_{i}\in\mathcal{X},\beta_{i}\in\mathcal{Y},i=1,\ldots,v$, and $v$ is size of the dataset including $\mathcal{X}$ and $\mathcal{Y}$. The total loss of SPN is:

where $\zeta_{mre}$ and $\zeta_{adv}$ are the weights of $L_{mre}$ and $L_{adv}$, respectively, which satisfy the condition: $\zeta_{mre}+\zeta_{adv}=1$.

Vision-based manipulation experiments are conducted to validate our proposed framework. The experimental platform used in our study includes a fixed D455 depth sensor, a UR5 robot manipulator, and various deformable objects shown in Fig. 6. The depth sensor receives the video stream, from which it computes the 3D shapes by using the OpenCV and RealSense libraries. In our experiments, only 3 DOF of the robot manipulator are considered, therefore, the control input $\mathbf{u}=[u_{x},u_{y},u_{z}]^{\intercal}\in\mathbb{R}^{3}$ represents the linear velocity of the end-effector; A saturation limit of $|u_{i}|\leq 0.01$ m/s, is applied for $i=x,y,z$. The motion control algorithm is implemented on ROS/Python, which runs with a servo-control loop of 10 Hz. A video of the conducted experiments can be downloaded from https://github.com/JiamingQi-Tom/experiment_video/raw/master/paper4/video.mp4

The proposed shape extraction algorithm is depicted in Fig. 7. The RGB image from the camera is transformed into HSV and combined with mask processing to obtain a binary image. $OpenCV/thinning$ is utilized with FPS to extract a fixed number of object points. The point on the centerline closest to the gripper’s green marker is chosen to sort the centerline along the cable. Then we obtain the 3D shapes by using the RealSense sensor and checking its 2D pixels. We adopt the method in [11] to compute the object’s contour. A surface is obtained in the similar way to the centerline, i.e., by sorting points from top to bottom, and from left to right. As 2D pixels and 3D points have a one-to-one correspondence in a depth camera, thus, our extraction method improves the robustness to measurement noise and is simpler than traditional point cloud processing algorithms.

In this section, ten thousand samples of centerlines, contours and surfaces with $N=64,64,32$, respectively, are collected by commanding the robot to manipulate the objects, whose configuration is then captured by a depth sensor. Such shaping actions are shown in Fig. 8, and visualized in the accompanying multimedia attachment. This data is used to evaluate the performance (viz. its accuracy and computation time) of our representation framework. For that, we calculate the average error between the feedback shape $\bar{\mathbf{c}}$ and the reconstructed shape $\hat{\bar{\mathbf{c}}}$ as follows $\operatorname{mean}(\sum{\left\|{{{\bar{\mathbf{c}}}_{i}}-{{\hat{\bar{\mathbf{c}}}}_{i}}}\right\|})$. The computation time is defined as the average of the overall processing time of all sample data among each method.

Fig. 9 shows that the larger the scalars $n,n_{x},n_{y}$ are, the better the fitting accuracy of LSM and MLS is. MLS fits better than LSM under the same condition, as MLS calculates the independent weight while LSM assumes that each node has the same weight. MLS works better in fitting contour because the parametric curve may not be continuous in the end corner, thus, the equal weight assumption of LSM is not suitable here. As the number of data points for surface is $N=32$, it does not satisfy the condition $N\gg(n_{x}+1)(n_{y}+1)$ for higher order fitting models (e.g., $n_{x}=n_{y}\geq 5$), thus, we only use $n_{x}=n_{y}\leq 4$. The results show that MLS performs better than LSM in the surface representation; Interestingly, MLS obtains satisfactory performance even with $n_{x}=n_{y}=1$. Fig. 9 also shows that larger $n,n_{x},n_{y}$ will also increase the computation time. MLS has a more noticeable increase, as it calculates the weights of all nodes while LSM calculates them once. The trigonometric approach is the fastest, polynomial and Bernstein follow, while Cox-deBoor is the slowest with the most iterative operations. The above analysis verifies the effectiveness of the proposed extraction framework, which can represent objects with a low-dimensional feature. Details of the curve fitting configuration is given in Table. I.

The data collected in Section VII-B is used to evaluate our proposed SPN. 80% of the data is used as the training set, and the remaining 20% of the data is used to test the trained network. We set $\delta=2$ for the centerline, contour, and surface parametrization. SPN is built using PyTorch and trained by an ADAM optimizer with a batch size of 500, and the initial learning rate set to 0.0001. RELU activation and batch normalization are adopted to improve the network’s performance. In this validation test, the robot deforms the objects with small babbling motions while a cardboard sheet covers parts of objects. Fig. 10 shows that SPN can predict and provide relatively complete shapes for the three types of manipulated objects. The accompanying video demonstrates the performance of this method.

This section aims to evaluate RTM (40) that approximates the deformation Jacobian matrix; The performance of the RTM estimator is compared with the interaction matrix estimator (IEM) in [15], and the unscented Kalman filter (UKF) in [31]. To this end, we introduce two metrics, i.e., $T_{1}$ and $T_{2}$, to quantitatively compare performances of these algorithms:

where $\widehat{\mathbf{s}}_{k+1}=\widehat{\mathbf{s}}_{k}+\hat{\mathbf{J}}_{k}\mathbf{u}_{k}$ is the approximated shape feature that is computed based on the control actions. Fig. 11 shows that RTM with $\eta=20$ provides the best performance for $T_{1}$ among the methods; This means that constraint $Q_{1}$ (37) enables RTM to learn from past data by adjusting $\eta$. Small values of $T_{2}$ reflect that RTM accurately predicts the differential changes induced by the DJM; This means that constraint $Q_{2}$ (38) helps to compute a smooth matrix $\hat{\mathbf{J}}_{k}$. As the proposed method incorporates the constraint $Q_{3}$ (39), thus, RTM can prevent singularities in the estimation of the Jacobian matrix, while IEM and UKF are prone to reach ill-conditioned estimations. We use RTM with $\eta=10$ in the following sections.

This section conducts four automatic manipulation experiments, labeled as Exp1, Exp2, Exp3, and Exp4, respectively. The target contour $\bar{\mathbf{c}}^{*}$ is obtained from previous demonstrations of the manipulation task, which ensures its reachability. A cardboard sheet is manually placed over the object to produce (partial) occlusions and test the robustness of our algorithm. The estimation methods in [15] and [31] are compared with the proposed receding-time model with MPC ($h=5$ and $h=15$). We label these methods as $\rm{CM}_{1},\ldots,CM_{4}$, and each method has been optimized to achieve a balance of stability, convergence, and responsiveness.

In addition to the feedback shape error $\|\mathbf{s}^{*}-\mathbf{s}_{k}\|$, we also compare $T_{max}$ (i.e., the number of steps from start to finish), $t_{d}$ (the steps from $\|\mathbf{s}^{*}-\mathbf{s}_{0}\|$ to 10% of this value), $t_{s}$ (the steps from 10% $\|\mathbf{s}^{*}-\mathbf{s}_{0}\|$ to the threshold value), and $d_{eff}$ (the total moving distance of the end-effector), [11, 23]. Fig. 12 shows the shaping motions of the manipulated objects toward the desired configuration (black curve), with the cardboard blocking the view at various instances. The results demonstrate that SPN can predict the object’s shape during occlusions and feed it back to the controller to enforce the shape servo-loop; This results in the objects being gradually manipulated towards the desired configuration. The error norm $\|\mathbf{s}^{*}-\mathbf{s}_{k}\|$ plots in Fig. 13 shows that $\rm{CM}_{3}$ provides the best control performance for the error minimization, with $\rm{CM}_{4}$ as the second-best, and $\rm{CM}_{1}$ and $\rm{CM}_{2}$ showing similar performance. A higher $h$ is helpful for feature prediction, yet, since we assume that $\hat{\mathbf{J}}_{k}$ is constant in the window period $h$, it may lead to inaccurate predictions and even wrong manipulation of objects. Therefore, $h$ should be chosen according to the performance requirements of the system.

From the $Q_{3}$ plots in Fig. 13, we can see RTM with $\eta=10$ provides the smallest value, which validates that RTM can enhance the manipulation feasibility and avoid shapes falling into the singular configurations. The black dashed lines represent the workspace constraints of $r_{x},r_{y},r_{z}$, on which we can see the end-effector’s trajectories in the Cartesian coordinate system; The results show that $\rm{CM}_{3}$ and $\rm{CM}_{4}$ remain within the workspace, while $\rm{CM}_{1}$ and $\rm{CM}_{2}$ may violate this constraint. Due to the adopted input saturation in our platform, all four methods can satisfy the control saturation constraint. Exp4 shows that our method has good universality, not only for the shape control, but also for traditional rigid object positioning.

A performance comparison of these experiments is given in Fig. 14. The results illustrate that our proposed shape servoing framework achieves the best performance relative to the manipulation speed $T_{max}$, response speed $t_{d}$, convergence speed $t_{s}$, and motion distance $d_{eff}$.