ViIK: Flow-based Vision Inverse Kinematics Solver with Fusing Collision Checking

Inverse Kinematics is a primitive of motion planning. To find a feasible trajectory or one satisfying the criterion, motion planners need to use a subset that is large enough to cover the universal set of configurations satisfying the target pose as the goal states. However, the classical IK solvers for 7+ DoF robots (e.g. TRAC-IK) output one configuration by each running. To obtain a large set of configurations, the classical methods need to run enough times with random start states. The learning-based IK solvers [6]-[8] utilize the neural networks for IK, but only can output one configuration for each target pose. [9]-[12] can output diverse configurations by using the generative models including Generative Adversarial Networks (GAN), Auto-Encoder (AE), and Normalizing Flows (NFs). IKFlow[11][12] has shown the promising results. The accuracy of solutions is within 10mm and 2 degrees. Moreover, the classical IK solver is speeded up significantly by seeding with IKFlow.

In practice, the IK solutions need collision checking before being goal states of motion planners, which means the executed times of collision checking might also be large. Therefore, we aim to develop a learning-based method that fuses IK and collision checking to speed up the preparation of motion planning. In this paper, we propose the flow-based Vision Inverse Kinematics solver (ViIK) that can output diverse collision-free solutions to achieve this goal.

Flow-based models are to model the target distribution as a transformation $F_{\theta}$ of the base distribution that is usually standard normal distribution:

Furtherly, the density of $x$ can be obtained by a change of variables:

Moreover, the density of $x$ conditioned on $y$ can be written as:

In practice, we often construct a neural network to fit the transformation $F_{\theta}$. The corresponding objective function is usually Kullback-Leibler (KL) divergence to minimize divergence between the flow-based model $p_{X}(x|y;\theta)$ and the target distribution $p_{X}^{*}(x|y)$, which can be estimated by Monte Carlo[16]:

where $\mathcal{X}=\{x^{(i)}\}_{i=1}^{N}$, $\mathcal{Y}=\{y^{(i)}\}_{i=1}^{N}$.

For 7+ DoF robots, the IK solvers output the samples in the sub-space of the configuration space (C-space) where the target pose ($p$) is the condition. We can, firstly, use a conditional distribution to approximate the sub-space, where the density can be denoted as $p_{C}(c|p)$. C-space can also be divided into two sub-spaces: collision-free space denoted as $C_{c-f}$ and collision space denoted as $C_{c}$. Further, conditional distribution can be used for collision checking, where the density can be denoted as $p_{C}(c|x_{c})$, where $x_{c}$ is whether collision-with-env occurs. Therefore, the collision-free configurations satisfying the target pose can be approximated by the two conditional distributions, denoted as:

where $x^{\prime}=(c,x_{c})$, $x_{c}$ is whether collision-with-env occurs. The first one $p_{C_{c-f}}(c|p)$ is to train models without negative samples where models might perform badly near the boundary. As for the second one $p_{C}(c|p,x_{c})$, the model has similar performance on position and angular error compared with $p_{X}((c,x_{c})|p)$, but performs badly on collision-sith-env and self-collision rates, as shown in Tab. I. In this paper, we use $p_{X}((c,x_{c})|p)$ as the target distribution. After adding self-collision in the same way, the target distribution is:

where $x=(c,x_{s-c},x_{c})$, $x_{s-c}$ is whether self-collision occurs. Moreover, to output collision-free configurations in different environments and perceive environments with RGB images, the multi-view images ($\mathcal{I}$) are condition, where the density is written as:

where $\mathcal{I}=\{I_{j}\}_{j=1}^{M}$, $M$ denotes the number of view.

Due to the large number of configurations required for motion planning, the efficient sampling of NF is suitable for IK. Moreover, for perceiving environments without structured maps, the RGB images need to be the input of NF. Therefore, we propose a Vision-conditioned flow-based model, named VisionFlow, as shown in Fig. 2. VisionFlow is a flow-based model that takes instruction (It’s the target pose in ViIK) and RGB images as conditions as shown in Eq. 7.

Issues that degrade the performance of flow-based models include: 1) the discrete data [14]; and 2) the target distribution is a manifold [15]. For the first issue, the discrete data, datasets are sets of samples from the true distribution. The sampling process can be seen as quantization. Training a flow-based model on the discrete data is to fit a continuous density model to a discrete distribution, which might lead the model to collapse into a degenerate mixture of point masses[14]. As shown in Eq. 7, the target distribution might be a manifold in the C-space by approximating IK solutions by a conditional distribution.

Two abovementioned issues can be sidestepped by dequantization. In this paper, we choose to implement the dequantization method, PaddingFlow[13], which is widely suitable and easy to implement. By referencing the results of IK experiments in PaddingFlow[13] we modify the PaddingFlow noise for the data used in this paper. Unlike the configuration, the self-collision and collision-with-env flags are boolean values. Thus, the Gaussian noise is added to the self-collision flag ($x_{sc}$) and collision-with-env flag ($x_{c}$):

where $a$, and $b$ denote the variances of data noise, and padding-dimensional noise respectively.

We compare two target distributions ($p_{2}$, and $p_{3}$) on position error, angular error, collision-with-env rates, and self-collision rates. The results (Tab. I) show $p_{2}$ performs better on all 4 metrics. Especially on collision-with-env and self-collision rates, $p_{3}$ performs badly, even similarly to the test set.

We compare the runtime of ViIK to the classical method, TRAC-IK+CC. The tolerance of TRAC-IK is set to $1\times 10^{-3}$ which is similar to the errors of ViIK shown in Tab. II. Moreover, Mesh Collider is used for collision checking in TRAC+CC. We let TRAC-IK repeatedly execute until the number of output solutions is the same as ViIK, $t_{ik}$ denotes the total runtime of TRAC-IK. Then we run collision checking for every configuration output by TRAC-IK, $t_{cc}$ denotes the total runtime of collision checking. The runtime of TRAC-IK+CC is:

In Fig. 4, we present the runtime of different numbers of solutions from 10 to 1000. The results show that ViIK outperforms TRAC-IK when the number of solutions exceeds 40. The runtime of 100 and 1000 solutions is about 1/2 and 1/17 compared to the classical method.

We evaluate ViIK on the test set with 150,000 random IK problems of the Panda manipulator against IKFlow and PaddingFlow. We let ViIK, IKFlow, and PaddingFlow output 1000 solutions for each target pose. In Tab. II, three models (ViIK-2, ViIK-5, and ViIK-10 trained on the data of 2, 5, and 10 scenes respectively) can output competitive solutions compared with existing learning-based methods. ViIK outperforms IKFlow and PaddingFlow on position error which is about 1/2 of IKFlow, but on angular error, ViIK performs worse than PaddingFlow.

We present ViIK rates of self-collision and collision with the environments on the test sets. We test self-collision and collision-with-env rates on the test set with 10,000 random IK problems of the Panda manipulator. For each target pose, ViIK outputs 1000 solutions. For different scenes, we evaluate ViIK on the same test set but with different images of each scene. We report self-collision and collision-with-env rates over 10,000 target poses, as shown in Tab. III. The self-collision rates of ViIK are lower than 2%, which is significantly lower than the test set. As for the collision-with-env rates, ViIK can perform well in most scenes but is not ideal in some hard scenes (Env-1, and Env-5).

We present the scenes used in this paper in Fig. 5. All scenes are chosen from the benchmark proposed in[17].

All models are GLOW-based[18] with 24 coupling layers. The training set has 5 million random samples in the C-space with the corresponding end-effector poses and 4882 sets of multi-view images of each environment. The batch size is 1024. The Adamw optimizer is used for training all models. The hyperparameters for training ViIK are shown in Tab. IV. Moreover, the multi-views used in this paper are presented in Fig. 6 with an example in Env-1.