# Implementing Forward Kinematics

This tutorial continues our quest to solve the problem of forward kinematics. After exploring a mathematical solution in The Mathematics of Forward Kinematics, we will see how to translate it into C# for Unity. The next tutorial, An Introduction to Gradient Descent, will finally show the theoretical foundations to solve inverse kinematics.

The other post in this series can be found here:

At the end of this post you can find a link to download all the assets and scenes necessary to replicate this tutorial.

#### Introduction

In the second part of this tutorial on Inverse Kinematics, The Mathematics of Forward Kinematics, we have formalised how a robotic arm moves. We started with a toy example, made by three joints. When in their resting positions, they assume the configuration seen in the diagram below:

In the diagram, the various represents the Cartesian coordinates or the -th joint. The local angles that indicates how much they rotate from their resting positions are labelled .

When joints rotate, we see the following:

The behaviour of this system has been summarised with the following statements:

• Rotation. The global rotation  of a joint is the sum of the rotations of all the previous joints:

• Position. The global position of a joint is given by:

Knowing all of the above, we can start thinking of a possible way to implement these behaviours in Unity.

#### GameObjects Hierarchy

Unity already comes with a way to implement all the requirements mentioned above: parenting. Setting a game object as a child of another one automatically inherit the position, the rotation and the scale.

If you are familiar with rigging, this should not surprise you. The bones that represent the joints of a humanoid character are parented in such a way that rotations and translations are inherited. The following image, from Unity Animation 3: Character Setup (by Michael Arbuthnot) shows a clear example of this.

While building your hierarchy of joints, you have to make sure that the robotic arm is in resting position when all the local Euler angles are set to zero. In a humanoid character, this usually corresponds to the standard T-stance seen in the picture above.

#### The Implementation

The parenting option in Unity is, de-facto, solving the problem of forward kinematics for us. Unfortunately this is not enough. We will see in next part of this tutorial, Inverse Kinematics with Gradient Descent, that we actually need a way to test the position of the end effector without actually moving the robotic arm. This forces us to re-implement this basic feature in Unity.

The first step is to store some information on each joint of the robotic arm. This can be done by adding a script, such as RobotJoint in the example below.

using UnityEngine;

public class RobotJoint : MonoBehaviour
{
public Vector3 Axis;
public Vector3 StartOffset;

void Awake ()
{
StartOffset = transform.localPosition;
}
}

You should add RobotJoint script to each game object that serves as a joint for your robotic arm. Every game object that you want to be rotated by the IK system should have one such script attached to it.

To simplify the calculations, we assume that each joint can only rotate around one its local axes: either X, Y or Z. We indicate that with a variable called Axis, which has a 1 in the position relative to the rotation axis. If this joint rotates around the Y axis, Axis would be (0,1,0). We will see how this allows us to avoid IF statements.

The actual IK code should be store in a different script, which will serve as an “IK manager”. You can put it inside the root of your robot arm, although it does not really matter where it is. Its purpose will be to perform both the forward and inverse kinematics. To do so, the manager script needs to know where the joints are. This will be possible by storing them inside an array or a list of RobotJoints, which will be called Joints.

The manager script will need a function, called ForwardKinematics. It takes as input an array of floats, called angles. The name is self-explanatory: angles[i] contains the local rotation for the i-th joint contains in the Joints list. The function returns the position of the end effector, in global coordinates.

public Vector3 ForwardKinematics (float [] angles)
{
...
}

The code is a straight forward implementation in C# of the position equation seen before. The rotate function is implemented with the handy Quaternion.AngleAxis.

Vector3 prevPoint = Joints[0].transform.position;
Quaternion rotation = Quaternion.identity;
for (int i = 1; i &lt; Joints.Length; i++)
{
// Rotates around a new axis
rotation *= Quaternion.AngleAxis(angles[i - 1], Joints[i - 1].Axis);
Vector3 nextPoint = prevPoint + rotation * Joints[i].StartOffset;

prevPoint = nextPoint;
}
return prevPoint;

❓ Need help with Quaternions?

#### Other resources

Become a Patron!
Patreon You can download the Unity project for this tutorial on Patreon.

Credits for the 3D model of the robotic arm goes to Petr P. A big thanks also goes to Maurizio Scuiar.

### 25 responses to “Implementing Forward Kinematics”

1. I can’t seem to understand what angles array is. Do we send it the angle it’s allowed to move in? (X, Y, Z etc?) Because it’s a float array.

1. Hi! Apologies for the confusion! You are right, angles can rotate around 3 axes (X, Y, Z). But for this solution, each joint can only rotate around ONE axis. And that angle is indeed contained in the angles array!

2. Hello, this is a great tutorial, thankyou very much for it, however I’m really confused at the moment. Nowhere in the tutorial, at least where I can find, mentions how each value for the angles[] is detirmined. I understand that it is an array of each joints angle along its respective axis, however I’m very unsure how to get this for each joint. Theres no way to hard code it since the angle could be any value at any time and on any given axis. And I can’t think of any way to generically get them since they can all be on different axes to eachother. Is the solution just to have the array initialise to all zero values and just make sure the robot begins in a zero position?

1. Hi Mitch,
Did you ever understand how the angles array works ? I’m having trouble as well.

1. You should define angles as an array of the same size of Joints and 0.0f as value for each one. InverseKinematics will update it in each Update

3. Hey Alan,
thanks so much for the tutorials!

The effector position should be equal to the last joint’s position right?
I cannot for the life of me get this code to work.

I have all of my Axis set to Vector3.Up.
My start positions are:
Joint[0].StartPosition = (0,0,0)
Joint[1].StartPosition = (2,2,0)
Joint[2].StartPosition = (4,4,0)

And yet somehow when all angles are 0,
ForwardKinematics returns (6,6,0)

Does that seem right?

1. Aha! I just realized that
StartOffset = transform.localPosition;
implies a parented structure. I was trying to implement this without parenting. For this to work I need to calculated StartOffset to be from the previous joint.

4. Hi, Alan, in which class the ForwardKinematics () function should be located?

5. Hey, Alan, great tutorial, I’ve been really interested in learning more about Inverse Kinematics and Procedural Animations and your tutorials have been my starting point. I just got a little confused on the code part, where is the ForwardKinematics () function supposed to go? Should it be in the RobotJoint class or somewhere else? And where does Joints [i] and angles[] come from? Thanks for the great work!

6. Had to do this whole thing. (assuming [code] makes code blocks)

[code]
public Vector3 ForwardKinematics (float[] angles, GameObject[] Joints) {

Vector3 prevPoint = Joints[0].transform.position;
JointIK[] JointScript = new JointIK[Joints.Length];
for (int i = 0; i < Joints.Length; i++) {

JointScript [i] = Joints [i].GetComponent ("JointIK") as JointIK;

}
Quaternion rotation = Quaternion.identity;
for (int i = 1; i < Joints.Length; i++)
{
// Rotates around a new axis
rotation *= Quaternion.AngleAxis(angles[i – 1], JointScript[i-1].Axis);
Vector3 nextPoint = prevPoint + rotation * JointScript[i].StartOffset;

prevPoint = nextPoint;
}
return prevPoint;

}
[/code]

1. Hey!
I usually assign them by hand to make sure the order is correct.
But yeah, that should work as well.

7. Really confused with Joints[]. I don’t know if I should reference it or just define it in script. Please help…

8. Could you please explain what Joints[] is? I inferred the RobotJoint class is supposed to have an array of RobotJoints, but it’s never written and as is the code will not compile.

Is there something that doesn’t compile?

1. Can you please just put the Joints[] variable inside the example of the code you gave instead of giving incomplete tutorial code and requiring people to download your package to go through the tutorial?

1. Wow you’re crazy dude.
Is this how you thank someone sharing his knowledge for free ?
This world…

1. May be, but incomplete knowledge is dangerous. :-p. I am also a little bit confused what is joints. I don’t wanned to download the project because i wanned to learn but don’t make incomplete tutorial. If you want, people should download it then don’t put code inside the tutorial. Shearing knowledge for money is called business, then make it as a business. Do not make tutorials.

2. Hi Rabi.
I did not write this tutorial with the intention of being obscure, or impossible to re-create without the package on Patreon. Quite the opposite, I tried to do my best to make it accessible and clear for everyone. I have hundreds of tutorials on my website, and virtually all of them can be fully recreated without the need to pay a single penny.
I believed the language used in the tutorial was clear, but it appears a few readers were confused by “Joints”. I have made a few corrections which I hope now makes much more clear how it should be used.
Ultimately, this is a rather advanced tutorial and despite my effort, not all developers might be able to reimplement this solution.

To address your final point: making tutorial is part of my business. And it is how I can afford to spend weeks, or even months of works writing tutorials. I find that argument rather disingenuous, as I could make more than 10 times what I currently make by simply selling all of my tutorials on the Asset Store.

9. I am having difficulty following this here. Does Forward Kinematics code go in your IKJoint class? Is the rotate code meant to be in its own function? Its kind of vague and it seems to me that the rotate code returns the position of the end point anyway?

1. Hi James!
The IK code doesn’t perform any rotation. It calculates the position of the end effector, given a set of angles.
I can use that to predict what the robotic arm is touching.
Then, using Gradient Descent to find a solution.

10. Vector3 nextPoint = prevPoint + rotation * Joints[i].StartOffset;

I may be wrong, but I think in this line local vector is added to global one. Missing conversion? Or am I missing something?

11. […] Part 3. Implementing Forward Kinematics […]

12. […] Part 3. Implementing Forward Kinematics […]

13. […] Part 3. Implementing Forward Kinematics […]

14. […] Part 3. Implementing Forward Kinematics […]