Optimization Based Inverse Kinematics

The problem we are solving here is geometrical inverse kinematics. Given an end-effector pose, the task is to figure out joint positions that put the end-effector as close as possible to the given one.

The  problem is formulated as a nonlinear optimization problem. 

The goal is to minimize the distance between the actual end-effector pose and the desired one. 

Given a redundant kuka manipulator with 7 DoFs, we can actually achieve a secondary task while satisfying the end-effector pose task.

Here, we simply define another distance task between the elbow link and a fixed external point. 

The solution differs from the previous one due to the secondary task as shown below.