This blog post will give you an overview of how I implemented an inverse kinematics solver in Jax and Mujoco for a 3DOF arm. The goal was to only focus on building a good algorithm for solving IK problems for this specific robot configuration. The more longterm goal is to be able to build out this robot in the real world.
As always we will start with some theory. Inverse kinematics (will be referred to as IK from now on) is one of the most fundamental concepts of robotics. IK aims to answer the questions “Given a desired end-effector position, which are the necessary joint configurations?”. Because this is such a fundamental problem, there are many ways to solve this problem, ranging from analytical solutions to numerical ones.
Since inverse kinematics has the word “inverse” in it, it hints at something about inverses. This is correct and the thing that is being inverted is the so called forward kinematics. The forward kinematics is a transformation $f(q)$ that takes the joint configurations and gives the end effector pose. This information is not that useful, it basically says, “if we move this joint, the end effector pose will also move”. However, the inverse is much more interesting. There we can get which joint configurations is required for a certain pose. IK is however a bit more than calculating the inverse of the forward kinematics. There are various methods that all have their advantages and disadvantages. This project uses an approach based on gradient descent.
$$ x = f(q)
$$
$$ \Delta x = (\partial f / \partial q) \Delta q $$
$$ \Delta q = (\partial f / \partial q)^{-1} \Delta x $$
Now that we have the gradient/jacobian of the joint configuration we can update the joint configuration using standard gradient descent like this:
$$ q_{k+1} = q_k + \alpha \Delta q_k $$
$$ q_{k+1} = q_k + \alpha (\partial f / \partial q)^{-1} \Delta x $$
In this case, $\Delta x = x_d - x$, where $x_d$ is the desired end effector pose and $x$ is the current end effector pose. We can now use it to minimize the error and solve the IK problem.
As mentioned, this was performed in a simulated environment, using my favorite simulator, Mujoco. I first began by creating a model of the arm using their XML syntax, nothing special really. The reason why I did not go for a premade industrial model like a Franka or a UR will be explained later on in the post.
Image of the robot arm model in Mujoco. Also shows the target, a blue sphere.
The goal is for the robot tip to touch the target (blue sphere). The arm has three joints, one that rotates around the Z-axis, one on top that which rotates around the Y-axis and another one rotating around the Y-axis. All the joints are spaced out 0.5 units from each other and so is the (red) end effector.
We can now get into implementing the algorithm!!!!
Actually, can we?? On purpose I left out a quite important part from the theory section. The thing I left out is of course the forward kinematics. In the theory section I only wrote it as function that took in joint angles and mapped it to an end effector position. However I did not explain how that function looked like exactly. The reason being is that I want to keep the theory part very general and something like forward kinematics would feel to abstract in that case.