Full Inverse Kinematics Solution is Time Consuming

[arjung128]

Hi,

First of all, thanks so much for making this work open-source! :)

My question is: is the following the best way to get full inverse kinematics solutions (as opposed to just one inverse kinematics step) for arbitrary batch sizes?

batch_size = 1
goalangles, goalposes = robot.sample_joint_angles_and_poses(batch_size)
x = robot.sample_joint_angles(batch_size)

goalangles = to_torch(goalangles.copy()).cuda()
goalposes = to_torch(goalposes.copy()).cuda()
x = to_torch(x.copy()).cuda()

start_time = time()
for i in range(10000):
    x = robot.inverse_kinematics_autodiff_single_step_batch_pt(goalposes, x) # , alpha=1e-1)
    fk_res = robot.forward_kinematics(x, solver='batchfk')
    print(i, np.linalg.norm(fk_res.cpu().squeeze()[:3] - goalposes.cpu().squeeze()[:3]), time() - start_time)

I'm doing this for the Franka robot, and just one full inverse kinematics solution seems to be converging but getting the error down to $<$ 1e-4 is quite time consuming, which is why I wanted to double check.

Any help will be much appreciated, thanks in advance!

[jstmn]

Hey there!

By 'full' i'm guessing you mean an accurate solution? i.e. <1mm positional error.

This package has three different functions for computing IK.

• inverse_kinematics_single_step_levenburg_marquardt(.): performs a batched, single IK step using levenburg-marquardt (this is experimental, I just added it and am not sure how well it performs)
• inverse_kinematics_single_step_batch_pt(.): performs a batched, single IK step using the jacobian pseudo inverse method
• inverse_kinematics_autodiff_single_step_batch_pt(.): performs a batched, single IK step using pytorch's automatic differentiation functionality
• inverse_kinematics_klampt(): runs IK until convergence on a single config using Klampt's ik solver

I've found that inverse_kinematics_autodiff_single_step_batch_pt(.) performs rather poorly, I wouldn't recommend using it (it was added as an experimental feature). If you have only a single config to optimize i'd recommend just using the klampt solver (inverse_kinematics_klampt()), or using tracikpy or something.

Can you tell me about your use case? This repo and the associated ikflow (https://github.com/jstmn/ikflow/, https://arxiv.org/abs/2111.08933) paper are all built around the problem of getting many ik solutions quickly.

[arjung128]

Hi @jstmn! Thanks so much for the prompt response and for the clarification about the different functions for computing IK.

My use case is: I have a large batch of IK problems to solve, which are all independent from one another. I have been using Pinocchio (https://github.com/stack-of-tasks/pinocchio) for this -- while Pinocchio is quite fast (ie ~6 IK solver runs (of 5000 IK steps each) per second), each run of IK uses one CPU and I'm bottlenecked by the number of CPUs when I try to parallelize (eg with 10 CPUs, I can only solve 10 IK problems at a given time). Thus, as an alternative, I was trying to explore GPU-based methods to parallelize more effectively. For example, if I'm able to do IK in batches of 5000 on a single GPU, even if each IK step takes a bit longer than what I'm seeing with Pinocchio, because of the extreme parallelization, the overall compute time may be much shorter.

Note that I'm not necessarily interested in diverse solutions -- just a solution which gets to the desired pose. Does my idea seem reasonable? Would this library be able to help investigate this? While inverse_kinematics_klampt seems to return the full IK solution, this doesn't seem to accept batches of inputs, hence making parallelization difficult -- is my understanding correct?

Thanks once again for all your help!

[jstmn]

It sounds like IKFlow is perfect for your use case :)

You can get 5000 approximate solutions (<1cm positional error) in ~25ms with this solver as it runs in parallel on the gpu. You can then refine the solutions to arbitrary accuracy using the inverse_kinematics_single_step_batch_pt() function in this package ( inverse_kinematics_single_step_levenburg_marquardt() is worth trying too as it likely will converge faster).

For an example of how to do this please see this script: https://github.com/jstmn/ikflow/blob/master/examples/example.py (you'll need to use either panda__full__lp191_5.25m or panda_lite_tpm for the model_name cli argument).

Runtime comparison from IKFlow paper:

" While inverse_kinematics_klampt seems to return the full IK solution, this doesn't seem to accept batches of inputs, hence making parallelization difficult -- is my understanding correct?" Yep, that's correct. inverse_kinematics_klampt solves for single ik solutions sequentially.