NODE IK: Solving Inverse Kinematics with Neural Ordinary Differential Equations
for Path Planning

Inverse kinematics is, as the name implies, the inverse of forward kinematics defined by a function $f:R^n\to SE\left(3\right)$, mapping joint configurations $q\in R^n$ to an end-effector pose $x\in SE\left(3\right)$, where $n$ is the number of joints. The methods for the IK problem can be divided into analytic and numerical approaches. An analytic approach directly solves the joint configurations using geometric relationships. Unfortunately, a geometry analysis for inverse kinematics of a robot with more than 6 DoF is challenging and often not feasible. On the other hand, numerical methods can be trivially implemented and used for all single-chain robot configurations. The Newton-Rhapson method [9] and sequential quadratic programming [3] are well-known algorithms for solving the inverse kinematics problem. However, these methods take a considerable time for a large number of solutions – a requirement for a smooth joint path through a Cartesian target path.

There can be infinite solutions for a single target pose for a redundant robot (one in which a typical arm has more than six DoF). Thus, introducing a latent variable $z\in R^n$, the unique solution to the inverse kinematics can be determined as $q=f^{-1}(x,z)$. Note that the latent variable has the same dimension as the configuration because NF require the same dimension for the invertibility of the function. In this paper and prior work [2], NF is utilized to transform between latent variable $z$ and joint configuration $q$.

The normalizing flows (NF) [17], which are used in prior work [2], are generative models with a tractable distribution for efficient density estimation and sampling in the complex distribution. Using a tractable probability density function, such as normal distribution, NF are used to estimate the complex target distribution. When a sample is drawn from a tractable distribution, for example, $x_t\sim N(0,I)$, a model with learnable parameters $f_{\theta }:R^D\to R^D$ transforms the sample $x_t$ to a desired state $x_c$ in the complex distribution:

Conversely, the training process proceeds through the inverse function of the model $g_{\theta }=f_{\theta }^{-1}$ , maximizing log-likelihood. Let $x_t,x_c$ be the random variables in tractable and complex distribution and $g_{\theta }:R^D\to R^D$ be an invertible function where $x_t=g_{\theta }\left(x_c\right)$. Then, $p\left(x_t\right)$ is known by tractable distribution, and the change of variable theorem enables the computation of exact changes in probability:

And the log probability is

The model parameter $\theta$ is updated to maximize $\log p\left(x_c\right)$.

The prior work IKFlow [2] modeled the transform functions by a composition of the invertible functions:

The model depth $N$ needs to be large enough for an expressive model. However, the number of parameters increases when the model’s depth $N$ increases.

A neural network for hidden dynamics $h_{\theta }$ representing ODE:

which is called Neural ODE [5], is known for its ability that estimates complex distribution using a relatively simple model. Starting from the initial value, which is used as an input layer, $z\left(t_0\right)$, the desired value $z\left(t_1\right)$ is the solution to the initial value problem at some time $t_1$:

A black-box differential equation solver can compute the desired value, automatically evaluating the hidden dynamics $h_{\theta }$ as needed with the desired accuracy. This structure lets the hidden dynamics $h_{\theta }$ form continuous vector fields for transforming from the initial state to the desired state. The gradients for training are computed using the adjoint sensitivity method. Let $L\left(z\left(t_1\right)\right)$ be a loss function for the output of ODE solver:

Utilizing the adjoint $a\left(t\right)=\partial L/z\left(t\right)$, the gradient with regard to $\theta$ for optimizing the hidden neural dynamics:

This neural ODE form can be used for NF structures, which is called continuous normalizing flows (CNF) [5]. Let $z\left(t_0\right),z\left(t_1\right)$ be the random variables in tractable and complex distribution as in Sec. 2.2  . Then (6) can be used as a transform function like (1). This form is easily invertible by reverting $t$. Thus, $h_{\theta }$ does not need to be bijective for NF. In addition, this structure can be seen as having infinitely deep recurrent layers, enabling the expressive model. Note that time $t$ does not indicate robot trajectory time but the degree of transformation from $z$ to $q$. We will describe $z(t=0)$ as $z$ and $z(t=1)$ as $q$ in the rest of the paper.

CNF do not require determinant computation of the Jacobian because the change in log-density follows the instantaneous change of variables formula [5]:

Therefore, the target log-density is

which is solved by ODE solver. To maximize the log-likelihood, the loss function is

Additionally, free-form Jacobian of reversible dynamics (FFJORD) [8] was proposed to reduce computation costs of the Jacobian trace of (9). This method utilizes Hutchinson’s trace estimator [10], which allows a scalable unbiased estimation of the log-density. While computing Tr costs $O\left(n^2\right)$, FFJORD can approximate the log-density at cost $O\left(n\right)$.

The proposed model exploits the aforementioned property of the NF and Neural ODE for lightweight and accurate IK prediction. The proposed method uses conditional CNF to learn the configuration distribution for a given Cartesian pose $x$. The proposed method is implemented on top of SoftFlow [12] architecture for conditioning and FFJORD [8] for fast training. NVIDIA Warp [15] is utilized to compute the forward kinematics using GPU resources.

The proposed model utilizes conditional CNF using neural ODE:

The hidden dynamics model are conditioned by $t$ and $x$ for the dynamics corresponding to the end-effector pose. During a training procedure, instantly generated dataset is utilized.

Joint configurations $q$ are sampled in uniform distribution within the joint range. And, the forward kinematics for the joints is computed online:

where $\underset{–}{q}$ and $\overline{q}$ are the lower and upper bounds of the joints. Then, the trace of Jacobian (9) and the target value $z\left(0\right)$ of the current network model is computed as following relationship:

The proposed model uses a normal distribution as a tractable base distribution: $z\sim N(0,I)$ to easily sample and calculate the log density $\log p\left(z\right(0\left)\right)$. The model is directly updated by (8) with the loss function (11).

The structure of neural dynamics model $h_{\theta }$ is shown in Fig. 3. Because CNF do not require Jacobian determinant, the structure of hidden dynamics can be simply designed. Every hidden layer is conditioned in affine form.

When acquiring the batch IK for $k$ targets, a set of latent variable $Z=[z_1,\dots ,z_{k-1},z_k{]}^T\in R^{n\times k}$ are randomly sampled in normal distribution and target poses $X=[x_1,\dots ,x_{k-1},x_k{]}^T\in SE(3{)}^{km}$ are set for the given path, where $m$ is the number of IK targets, e.g., dual end-effector targets for humanoid robots. The output of the NODE IK is the corresponding joint configurations $Q=[q_1,\dots ,q_{k-1},q_k{]}^T\in R^{n\times k}$. Fig. 2 illustrates this relationship. This process is a one-time calculation taking advantage of GPUs.

When generating IK solutions for the given Cartesian path, $z$ should change continuously. The same or continuous change of $z$ is required for the continuous joint solution path. However, because the NF can learn multi-modal distribution, the path can be discontinuous even with the continuous change of the input variables. Therefore, after batch generation, every path is checked whether the path is continuous.

The proposed method and IKFlow were compared in accuracy, prediction time, and the number of model parameters.

The accuracy was measured using 7 DoF Franka Emika Panda robot. Model hyperparameters for IKFlow [2] were set to 1024 wide network width, 12 coupling layers as they claimed the best performance with these parameters with the robot. The hidden dynamics of NODE IK comprised four 1024 width layers. The accuracy was evaluated from 1000 sampled Cartesian target poses $x$ and the predicted 250 joint solutions for each target pose as in the experiments of previous work [2]. By passing the joint solutions to the forward kinematics function, the end-effector pose of predicted joint solutions $\hat{x}$ was computed.

NODE IK was able to learn faster and more accurately than IKFlow with the same number of data passed to the network during the training process. Fig. 4 illustrates the position error of the end-effector poses $||x_p-{\hat{x}}_p|{|}_2$. The position error of NODE IK was approximately half compared to that of IKFlow. Fig. 5 shows the orientation errors, which is quaternion geodesic distance between two poses. The orientation error of NODE IK was much lower than IKFlow. Outlier data with a large error existed among the generated solutions by IKFlow, which adversely affected the average value.

Although NODE IK had high expressive power, it required a few parameters. The number of model parameters of NODE IK was 3,316,320 for achieving the sub-centimeter error, while that of IKFlow was 50,934,364. NODE IK had 6.5 % parameters compared to IKFlow.

However, NODE IK took more time for prediction than IKFlow because multiple prediction steps of the hidden dynamics are necessary for the ODE solver. System setup for the evaluation was with Intel i7-10700K CPU and NVIDIA RTX 2080 Ti GPU. NODE IK and IKFlow took 85.93 ms and 14.97 ms, respectively, to find 1000 IK solutions. Still, the IK generation speed was faster than that of TRAC-IK [3], which is a numerical IK solver. Numerical solvers, TRAC-IK (C++) and Klampt IK (Python) took 1 s and 2.6 s, respectively, for 1000 IK solutions.

Path generation utilizing the advantages of batch IK was performed to evaluate the performance of the proposed model. Franka Emika Panda and TOCABI were used for the path generation. TOCABI is a 33 DoF humanoid robot, but, only upper body joints (arm and waist have eight and three joints, respectively) are used for IK. Target paths are given arbitrarily, which are circular paths. Orientation of the end-effector was fixed along the path. $z$ was sampled in $N(0,I)$ once and all IK solutions were acquired using each target pose and the same $z$. The discontinuous paths were rejected. Path errors are the average of position and orientation errors at each target pose. Especially, NODE IK was able to learn dual target IK for humanoid robots. 19 joints are used for dual target IK, which includes three shared waist joints.

The average path errors are listed in Tab. LABEL:tab:path_errors   . Overall, NODE IK showed less error than IKFlow. As the IK complexity increased, the errors increased. A path generated from NODE IK is shown in Fig. 6 as a qualitative result. The position and orientation errors differed according to the targets, but the relative change was small. Especially, the orientation error was barely changed. NODE IK had better performance for learning orientation conditions than IKFlow.