Solving Inverse Kinematics with Dual Quaternions and Polynomial Regression

Introduction

Inverse Kinematics determines the joint angle required to instruct robotic manipulators effectively to achieve a desired end-effector position. Current methods for solving Inverse Kinematics, including geometric, numeric, and differential approaches, each present limitations. Geometric methods offer a close-formed solution but become very complex as the degrees of freedom of the system increase. Numeric methods can be computationally expensive and time-consuming due to the iterative nature of the optimization requirements. Differential methods are plagued with singularities and require significant estimation through Taylor Series expansion to consider. These issues pose a problem for robotics and Internet of Things (IoT) applications, where low power consumption, weight, and cost are critical. Here I explore the use of machine learning to overcome these challenges.

This blog post is a tutorial explaining my notebook on GitHub.

Forward Kinematics

Forward Kinematics is mathematically solving for the position and orientation of a manipulator’s end-effector. It is usually far easier to solve than its inverse counterpart. I will use dual-quaternions to model the forward kinematics that we will use to generate data for training our model.

Quaternions

To understand dual quaternions, we need first to understand quaternions. Quaternions are an efficient way to represent rotations in three-dimensional space.
They extend the complex number system and are expressed in the form:

(1)  

A quaternion can be further broken down as a scalar () and a vector () part.

Quaternion Multiplication

Quaternion multiplication is not commutative, meaning that the order of multiplication matters. Given two quaternions:

their product is calculated as:

(2)  

This multiplication allows us to represent each joint angle’s combined effect on the end-effector pose.

Axis-Angle to Quaternion Conversion

We represent rotations as an axis of rotation (a unit vector ) and an angle of rotation around that axis. This is known as the axis-angle representation. We can use the exponential map to convert from an axis-angle to a quaternion.
The exponential map for quaternions is defined as:

(3)  

Applying this to the axis-angle representation, we get:

(4)  

Dual Quaternions

Dual quaternions extend the concept of quaternions to encode both rotation and translation.
We do this by introducing dual numbers, which have the form:

Here, and are real numbers, and is the dual number with the property that its square is zero.
A dual quaternion is then defined as:

(5)  

This allows us to encode a rigid transformation’s rotational and translational components.

Dual Quaternion Multiplication

Dual quaternion multiplication extends quaternion multiplication to incorporate both rotation and translation.
Given two dual quaternions:

where and are the real componenets, and and are the dual components, their product is:

(6)  

Advantages of Dual Quaternions

Dual quaternions offer several advantages in forward kinematics:

• Singularity-free: Dual quaternions do not suffer from singularities, which can cause computational issues and ambiguities.
• Compactness: They are an efficient way to represent both rotation and translation in a single entity.
• Efficiency: Dual quaternion multiplication allows for efficient chaining of transformations.

Implementation

We will use Sympy to implement dual quaternions. Sympy is a symbolic math Python Library. While Sympy has classes for quaternions, it does not for dual quaternions. So, I used a Sympy Matrix to hold the dual quaternions and implemented a custom multiplication function.

def dualq_mul(args):
    """
    Multiplies a list of dual quaternions.

    Args:
    args: A list of dual quaternions in Sympy Matrix format, 
          where each dual quaternion is a 2x1 matrix with the 
          real and dual parts as its elements.

    Returns:
    The product of the dual quaternions as a Sympy Matrix in expanded and simplified form.

    Raises:
    Exception: If less than 2 dual quaternions are provided.
    """


    if len(args) < 2:
        raise Exception("Need at least 2 dual quaternions to multiply")

    result = args[0]
    for i in range(1, len(args)):
        print(f"Multiplying {i + 1} of {len(args)}")

        dq_i = args[i]

        r1 = result[0]
        d1 = result[1]

        r2 = dq_i[0]
        d2 = dq_i[1]

        real_part = r1.mul(r2)
        dual_part = (r1.mul(d2)).add(d1.mul(r2))

        result = sympy.Matrix([real_part, dual_part])
        result = dualq_piecewise_expand(result)

    result = dualq_piecewise_trigsimp(result)
    return result

The above function takes in a list of dual quaternions in Sympy Matrix format, and loops through them while multiplying (See eq 6) the previous result with the next index. I wrote the simplification helper functions dualq_piecewise_expand() and dualq_piecewise_trigsimp() to simplify each component of the dual quaternion in parallel with Ipyparallel and Sympy’s simplification functions.

We start with joint and link definitions. When you create the hip, which has three degrees of freedom, you use a separate joint configuration for each degree of freedom. So, the first three joints all make up the hip. Each index of the joints list = [axis of rotation, translation vector, end_effector boolean, constraints].

joints = [
    [[0,1,0],[0, 0, 0 ], False, (np.deg2rad(-100), np.deg2rad(30))],
    [[1,0,0],[0, 0, 0 ], False, (np.deg2rad(-40), np.deg2rad(20))],
    [[0,0,1],[0, 0, 0 ], False, (np.deg2rad(-50), np.deg2rad(40))],
    [[0,1,0],[0, 0, -400], False, (np.deg2rad(0), np.deg2rad(150))],
    [[0,1,0],[0, 0, -450 ], False,  (np.deg2rad(-30), np.deg2rad(40))],
    [[0,1,0],[230, 0, 0], True, ()]]

Inspired by a human leg, this model uses angle constraints as found on Physiopedia.com. Our zero coordinate system is located at the hip. Because of this, the hip has no translation vectors. It has three joints. The knee translates 400 millimeters in the negative z direction. The ankle is located 450 millimeters further down the z-axis. The end-effector, located on the toes, translates 230 millimeters along the x-axis. Each joint’s axis of rotation has a value of 1, represented by the axes on which it rotates.

dq_joints = []
symbol_idx = 0
target_symbols = []

for joint in joints:
  if joint[2]:
    r = sympy.Quaternion.from_axis_angle(joint[0], 0)
  else:
    angle = sympy.symbols(f'theta_{symbol_idx}', real=True)
    symbol_idx += 1
    target_symbols.append(angle)
    r = sympy.Quaternion.from_axis_angle(joint[0], angle)

  t = sympy.Quaternion(0, joint[1][0], joint[1][1], joint[1][2])
  d = sympy.Rational(0.5) * t.mul(r)
  Q = sympy.Matrix([r, d])
  Q = dualq_piecewise_simplify(Q)
  dq_joints.append(Q)

The above loop builds a dual quaternion from each of the joint definitions. We use the Sympy symbols to create the angle variables for each angle, named ‘theta_#’. Depending upon whether the joint is the end-effector or not, we either use an angle variable or 0 as the angle of rotation in the axis-angle representation, then build a Sympy Quaternion from the axis angle with Sympy’s built-in conversion which uses the exponential map. From there we make the translation quaternion and the dual component of the dual quaternion as in equation 5.

foot_pose_dqi = dualq_mul(dq_joints)
foot_position_FK = (2 * foot_pose_dqi[1].mul(foot_pose_dqi[0].conjugate()))
foot_position_FK = foot_position_FK.to_Matrix(vector_only=True)
foot_orientation_FK = foot_pose_dqi[0].to_Matrix(vector_only=False)

foot_position_FK_lambda = sympy.lambdify(target_symbols, foot_position_FK, modules=['numpy'], cse=True, docstring_limit=0)
foot_orientation_FK_lambda = sympy.lambdify(target_symbols, foot_orientation_FK, modules=['numpy'], cse=True, docstring_limit=0)

Now that we have each joint as a dual quaternion, we multiply them from base to end-effector. Remember that dual quaternion multiplication is noncommutative. From there, we deconstruct the final foot pose using equation 5. The translation quaternion will give you the end-effector position by taking the vector part and we will leave the orientation in quaternion format for the end-effector orientation. We keep the real part representing the end-effector’s orientation in quaternion form to avoid singularities near zero.

Moving forward, we will no longer be using Sympy and will be working with Numpy and Jax. So we want to convert our Sympy variables to Numpy-supported functions for data generation. We used sympy.lambdify to create a Python helper function with the joint symbols we created earlier as parameters. This will allow us to call foot_position_FK_lambda(theta_0, theta_1, theta_2, theta_3, theta_4, theta_5) and compute the end-effector’s position from the angles. We can do the same to derive the rotation quaternion representing the final end-effector orientation. We will take advantage of Sympy’s Common Sub Expression generation and don’t need to produce any docstrings.

Data Generation

Now that we have an efficient way of generating our end-effector position coordinates and orientation quaternion, we want to generate end-effector position and orientation data along with their matching joint angles. To ensure we have the most coverage with the fewest points, we will use Latin Hypercube Sampling (LHS).

Latin Hyper Cube Sampling

The Latin Square above best defines how LHS works, where each row and column must contain one value of the allowed parameters with no duplicates. LHS extends this concept to higher dimensions. Instead of a square, we have a hypercube; instead of numbers, we have ranges of parameter values. In our case, the angles representing the current angle of each joint in the manipulator will be combined.

LHS first divides each parameter’s range into N equally probable intervals, where N is the sample size. It then randomly selects a value from each interval for each parameter, ensuring that every interval contains every parameter with no duplication. This gives us a uniform distribution
of angle values to generate our features.

Implementation

from scipy.stats import qmc

n = 10000

data = pd.DataFrame()

param_ranges = {}

for i in range(len(target_symbols)):

  param_ranges[target_symbols[i]] = joints[i][3]

sampler = qmc.LatinHypercube(d=len(param_ranges))
samples = sampler.random(n=n)
scaled_samples = np.zeros_like(samples)


for i, (param_name, (lower, upper)) in enumerate(param_ranges.items()):
    scaled_samples[:, i] = qmc.scale(samples[:, i].reshape(-1, 1), lower, upper).flatten()

We will use the scipy.stats.qmc.LatinHyperCube class to create a uniform distribution of angle combinations. First, we capture the joint constraints from the previous list of joints and match them with their symbol. We then create a sample in the unit hypercube with the dimensions equal to the number of joints in the system. Then we define the number of samples we want from the sampler (10000). Finally, we scale the values based on the angle constraints we defined earlier.

for i in range(n):

    position_x, position_y, position_z = foot_position_FK_lambda(*scaled_samples[i])
    orientation_w, orientation_x, orientation_y, orientation_z = foot_orientation_FK_lambda(*scaled_samples[i])
    feature_symbols = ['position_x', 'position_y', 'position_z','orientation_w', 
                        'orientation_x', 'orientation_y', 'orientation_z']
    headers = target_symbols + feature_symbols
    row = np.concatenate((scaled_samples[i], position_x, position_y, position_z, 
                          orientation_w, orientation_x, orientation_y, orientation_z))
    row = pd.DataFrame([row], columns=headers)

    data = pd.concat([data, row], ignore_index=True)

Now that we have the random angle combinations generated by the Latin Hypercube, we use those angles to generate the position and orientation values for each sample’s data.

Data Visualization

If you check the min and max for each angle from the above data.describe() function, you will see the radian representation of our constraints.

On the top left of the pairplot, you can see the uniform angle distribution from the Latin Hypercube Sampling. The top right shows the non-linear relationships between the features (position and orientation data) and the targets (angles). We can also see the non-linear relationships between the interactions of the features. The pair plot of the data gives a ton of insight into how the data interacts.

We can see the sample space in 3D above. This shows us that we have the right range and domain of our sample space. Given that we are modeling a human leg, we can see that the constraints are correct. We can also plainly see what we intuitively already knew. We know that x, y, and z depend on each other, and any change in any of those will change everything else in the system. We see the non-linear relationship of these changes.

Polynomial Regression

Due to the non-linear interaction between the targets and features and the non-linear interaction between the features, we have to figure out a way to capture all of those details. We could write a Neural Network, but we want something simple and easy for a Raspberry Pi to infer. So we look to a polynomial regressor.

Preprocessing

The secret to a polynomial regressor is in the feature engineering. To expand the features using polynomial expansion is simple. First, you have your bias term, then you have the combination of all available features based on the degree chosen.

(7)  

Keep in mind, this is an expansion of features and we have very few features initially. For my model, the degree 5 expansion increased the 7 features to 792 features. You have to be careful at high degrees of polynomial expansion because it would normally create too much complexity and overfit. This model is designed for a specific leg configuration and is not meant to generalize to all possible leg lengths and configurations. Since we only need accurate inverse kinematics for this particular robot, we can prioritize fitting the training data as closely as possible, even if it means the model wouldn’t perform well on other leg designs.

The degree of the polynomial is the primary hyperparameter in this model. I manually tuned this hyperparameter by training the model with degrees ranging from 1 to 6. I found that a degree of 5 provided the best fit, as increasing it to 6 resulted in a decrease in performance. While a more exhaustive grid search could be performed, manual tuning was sufficient in this case due to the single hyperparameter and the clear trend in the model’s performance.

Model

Once the features are expanded, we can use a simple linear model. I started with a standard Least Squares Linear Regressor. Since this model is specifically tailored to this leg configuration and not meant for general use, I prioritized fitting the training data as closely as possible. The initial results were excellent, so I didn’t feel the need to add regularization (like Ridge or Lasso regression) or explore other types of linear models.

Implementation

Most of the classes I wrote here are already available in Scikit-Learn. I chose not to use them because many schools will teach Scikit-Learn and all the parameters for its classes without really teaching the Machine Learning information itself. So I wanted to implement them myself.

I also wanted to learn more about Google Jax. Google Jax is an open-source Python library developed by Google ” for accelerator-oriented array computation and program transformation, designed for high-performance numerical computing and large-scale machine learning.” The following classes make use of Jax’s Just-in-Time Compilation (jit). This allows Jax to pre-compile a function to be executed by XLA.

class polynomial_transformer:
  def __init__(self, degree=2, include_bias=True):
    self.degree = degree
    self.include_bias = include_bias

  @staticmethod
  @partial(jit, static_argnums=[1, 2])
  def __expand(Xp, include_bias, degree):
      n_samples, n_features = X.shape
      combinations = []
      for deg in range(1, degree + 1):
        combinations.extend(itertools.combinations_with_replacement(range(n_features), deg))

      n_samples, n_features = Xp.shape
      n_output_features = len(combinations)
      X_new = jnp.empty((n_samples, n_output_features), dtype=jnp.float64)
      for i, index_combi in enumerate(combinations):
        X_new = X_new.at[:, i].set(jnp.prod(Xp[:, index_combi], axis=1, dtype=jnp.float64))

      if include_bias:
        X_new = jnp.c_[jnp.ones(n_samples), X_new]
      return X_new

  def transform(self, X, y=None):
    X_new = self.__expand(X, self.include_bias, self.degree)
    return X_new

Most of the code in the polynomial_transformer is self-explanatory. You first have the constructor with the degree and whether to include the bias. The transform function calls the __expand function. The __expand function is where all the work is done. First, we created a list of combinations for each degree using itertools.combinations_with_replacement() extending the list as we went. Second, we loop through that list and obtain the products of the combinations for each column in the data.

The __expand function is the most computationally expensive function in the whole model and would normally take around fourteen seconds to complete without the Jax transformations. By using jit compiling, each additional call to __expand takes less than a second to complete.

To accomplish this, we use the functools.partial decorator. The partial decorator with the jax.jit function and the static_argnums decorator tells Jax to compile the function for XLA and consider the include_bias and degree parameters as static.

class Standard_Scaler:
    def __init__(self):
        self.mean = None
        self.std = None

    @staticmethod
    @jit
    def __j_fit(X):
      mean = jnp.mean(X, axis=0)
      std = jnp.std(X, axis=0)
      return mean, std

    def fit(self, X):
      self.mean, self.std = self.__j_fit(X)
      return self

    @staticmethod
    @jit
    def __j_transform(X, mean, std):
      X_scaled = (X - mean) / std
      return X_scaled

    def transform(self, X):
        X_scaled = self.__j_transform(X, self.mean, self.std)
        return X_scaled

    def inverse_transform(self, X_scaled):
        X = (X_scaled * self.std) + self.mean
        return X

The standard scaler is also explanatory. We use the same closure technique to use jit within the class.

class linear_regressor:
  def __init__(self, fit_intercept=True):
    self.weights = None
    self.bias = None
    self.fit_intercept = fit_intercept

  @staticmethod
  @partial(jit, static_argnums=[2])
  def __j_fit(X, y, fit_intercept):

      X_mean = jnp.mean(X, axis=0)

      y_mean = jnp.mean(y, axis=0)

      if fit_intercept:
        X_centered = X - X_mean
        y_centered = y - y_mean
        weights = jnp.linalg.lstsq(X_centered, y_centered)[0]
      else:
        weights = jnp.linalg.lstsq(X, y)[0]


      bias = y_mean - jnp.dot(X_mean, weights)
      return weights, bias

  def fit(self, X, y):
    self.weights, self.bias = self.__j_fit(X, y, self.fit_intercept)
    return self



  def predict(self, X):
    return jnp.dot(X, self.weights) + self.bias

Finally, our linear regressor is standard fair. We use least squares to train and return the mx + b for predictions. As with everything else, we use jit to precompile for XLA and set the fit_intercept as a static argument.

Results

X_train_jnp = jnp.array(X_train.values, dtype=np.float64)
y_train_jnp = jnp.array(y_train.values, dtype=np.float64)

X_test_jnp = jnp.array(X_test.values, dtype=np.float64)
y_test_jnp = jnp.array(y_test.values, dtype=np.float64)

scaler = Standard_Scaler()
scaler.fit(X.values)
X_train_jnp = scaler.transform(X_train_jnp)
X_test_jnp = scaler.transform(X_test_jnp)

poly = polynomial_transformer(degree=5, include_bias=True)
X_train_poly = poly.transform(X_train_jnp)

lr = linear_regressor(fit_intercept=True)
lr.fit(X_train_poly, y_train_jnp)

y_train_pred = lr.predict(X_train_poly)

X_test_poly = poly.transform(X_test_jnp)
y_pred = lr.predict(X_test_poly)

y_pred = np.asarray(y_pred)
y_train_pred = np.asarray(y_train_pred)

print(f'Training set score: {r2_score(y_train, y_train_pred):.3f}')
print(f'Test set score: {r2_score(y_test, y_pred):.3f}')
print(f'MSE: {mean_squared_error(y_test, y_pred):.3f}')
print(f'MAE: {mean_absolute_error(y_test, y_pred):.3f}')

Training set score: 0.999
Test set score: 0.999
MSE: 0.000
MAE: 0.012

Training is fairly simple, I scaled the data using a custom standard scaler, expanded the features with the poly_transformer, and then fit the model. The results were fantastic (almost perfect), but you have to be careful with Polynomial Regressors. They can be subject to overfitting, especially at high degrees (degree=5) of expansion like I have. So we turn to cross-validation.

@jit
def fit_fold(X, y, test_index, train_index):
  lr_cv = linear_regressor(fit_intercept=True)
  X_train, X_test = X[train_index], X[test_index]
  y_train, y_test = y[train_index], y[test_index]
  lr_cv.fit(X_train, y_train)
  y_pred = lr_cv.predict(X_test)

  return y_test, y_pred



def cross_val_score_jax(X, y, k=5, random_state=None):

  key = random.PRNGKey(random_state) if random_state else random.PRNGKey(0)
  scores = []
  all_preds = []
  n_samples = len(X)


  shuffled_indices = random.permutation(key, jnp.arange(n_samples))
  fold_size = n_samples // k
  folds = [shuffled_indices[i * fold_size:(i + 1) * fold_size] for i in range(k)]

  for i in range(k):
    test_index = folds[i]
    train_index = jnp.concatenate([fold for j, fold in enumerate(folds) if j != i])
    y_test, y_pred = fit_fold(X, y, test_index, train_index)
    score = r2_score(np.asarray(y_test), np.asarray(y_pred))
    scores.append(score)

  scores = jnp.array(scores)
  mean_score = jnp.mean(scores)
  std_score = jnp.std(scores)

  return scores, mean_score, std_score

Scores: [0.99849085 0.99848519 0.99851316 0.99861235 0.99844522]
Mean score: 0.999
Std: 0.000

Cross Validation tests your model on different groupings of unseen data ensuring that the model can predict all data as though it has never seen it before. I used a CV of five. This breaks the data up into 5 randomized groups called folds. With each iteration of the loop, we train with 4 folds and predict on the 5th, ensuring that we can predict using each fold separately. Then we take the mean of all the r2 scores of each loop.

As with initial testing, the results are fantastic. We have an almost perfect prediction.

Discussion

Redundant Manipulators

One limitation of this method is that it may not be suitable for manipulators with significant redundancy. In robotics, redundancy occurs when a manipulator has more degrees of freedom than is necessary to perform a given task. This means there can be multiple joint configurations that achieve the same end-effector position and orientation.  

With its limited range of motion in each joint, the human leg does not exhibit significant redundancy. This allows the polynomial regression model to accurately predict joint angles based on the desired end-effector pose. However, the model may struggle to choose the “best” joint configuration from the many possibilities in a redundant manipulator.

To address this limitation, future work could explore incorporating:

• Current State Data: The model could use the current joint angles of the manipulator as additional input features. This would help it to choose a new configuration that is close to the current one, minimizing unnecessary movement.  
• Secondary Objectives: In addition to achieving the desired end-effector pose, the model could be given secondary objectives, such as minimizing joint torques, avoiding obstacles, or maximizing manipulability. These objectives would help to narrow down the possible joint configurations and choose one that is optimal for the given task.  

By addressing the issue of redundancy, the method could be extended to a wider range of robotic manipulators.

Performance Considerations

While this method has shown promising results in terms of accuracy, further testing is needed to assess its performance on a Raspberry Pi. My goal is to identify the inverse kinematics method that performs best on this platform, considering its limited computational resources and battery life.  

As I continue to explore and implement different inverse kinematics techniques, I plan to conduct comparative performance testing. This will involve evaluating factors such as:

• Computation time: Measuring the time taken to compute the inverse kinematics solution for a given end-effector pose.
• Memory usage: Assessing the amount of memory required to store the model and perform the calculations.
• Power consumption: Analyzing the power consumption of the Raspberry Pi while running the inverse kinematics algorithm.

By comparing these metrics across different methods, I aim to identify the most efficient and practical approach for solving inverse kinematics on a Raspberry Pi. This will help ensure that the chosen method can be deployed effectively in real-world robotics scenarios where resources are limited.

The full Colab Notebook can be found here.