# Forward kinematics¶

The point of forward kinematics is to calculate the position of the endpoints on the leg based on the actual state of the robot and its dimensions.

Usually, we are interested in finding the position of the tip point on the leg (the point which touches the ground). Our state is defined as a set of angles for the joints of the leg.

More formally, we are interested in a function that returns coordinations of the tip of the leg given angles and a set of attributes for the leg .

We decided to use a standard cartesian coordinate system to represent points and quaternions to represents rotations in space.

To properly define our context, we define quaternion Q representing the rotation of angle around axis as follows:

The rotation of quaternion Q applied to quaternion (or point) X results in X’, which represents X rotated angle around axis

## Segment formula¶

We can devise a specific formula for each type of leg or robot. However, that is impractical for the implementation of the robot. We decided to use one generic formula to define just one segment of the robot’s leg - it represents only one rotational joint and attached linkage. It is expected that leg is composed of multiple different segments. This should cover the majority of the estimated use cases.

This formula is designed in a way, that it can be composed easily to represent the entire leg.

Formula represents a segment of leg, that has something attached to it and that is itself attached to something. These attachments represent coordinate system of the segment and coordinate system of what is attached to the segment.

Given that x represents a point in coordinate system relative to what is attached to the segment, the function tranforms the coordinate to the coordinate system of this leg segment. If we transform , we specifically get where is the tip of the segment relative to it’s base. Parameters represents dimensions of the leg, is state of the leg.

and represents the rotational joint of the segment that is present at the end of the segment. Following segments attached to this segment are attached directly to this rotational joint.

and represents offset of the rotational joint from the base of the segment. Key is to realize that rotational joint is rotated by and than it is moved by offset (where is vector in cartesian coordinate system).

## Leg formula¶

The segments are composed in chain of rotational joints. Except for the last segment, which is just a point relative to it’s base. The coordinate of the point represents tip of the leg, which is point on the model of the leg at which the leg should step on.

# Inverse kinematics¶

The forward kinematics is simple math, what is more problematic is an abillity to get an inverse of the function. In what state the leg has to be, so it’s tip is at position ?

There are specific formulas to get this solution for simple robotic arms, this get’s however extensively diffcult in case of more complex setups. Given that we want somehow generall code, we decided to find a solution to IK for the segment-based approach we used for the FK.

The simplest algorithm used in literature for this is based on derivation of the FK function - Jacobian. The idea expects that we have an estimate (or starting) position for the solution, and we iterativelly improve are solution for the problem - numeric method.

The numeric methods defines a step of the computation, which is repeated until limit of repetitions is met or the solution is close enough. The step is fairly simple for the basic method:

represents target point we would like to position the arm at, is jacobian of the formula given angles Note that we are talking about Jacobian which represents derivation by the angles of the leg.

Given that in our case can to analytically found the jacobian for the FK formula, we believe that this is the right approach for us. In the rest of this section, we properly described how we can calculate the jacobian.

At first we show simple derivations for quaternions, in the rest of the document we expand on it to show derivation of the formula for FK/

## derivation of one leg segment¶

At first phase we will devise a derivation of one segment by it’s own angle. Keep in mind that axis,P,Q and x are unrelated to the angle itself.

To simplify this formula, we will introduce substitution that will be used:

W is quaternion that uses angle which we use for derivation. This simplifies the formula a bit:

Given the rule about derivation of quaternions rotations described above, we can make one step for free.

To continue the work on this, let’s process only the inner part of the equation:

In this equation, we are now only interested in the left part in summation, as that is the last problematic thing. We also want to only focus on the left part of the multiplication, as the right one is trivial.

Here we know that derivation of by is zero, as the x is unrelated to that angle. This implies that the right side of the sum is 0, so we can simplify:

## derivation of FK for specific leg¶

At first phase, let’s define how FK for some specific leg looks, 3DOF:

The jacobian - derivation of this equation is defined as follows:

We will solve this for each derivation separatedly, idea is that if we solve this on equation of specific leg, we can abstract it for more general case.

### derive by ¶

The equation for the first derivation looks like this:

As first step, we use substitution for the recursive calls, keep in mind that they do not contain - we do not have to derivate them, derivation is zero. This fullfills our original assumption about x.

Given that we know the result:

### derive by ¶

The equation for the second derivation looks like this:

For analysis of this equation, we will work with and substitutions for the f function:

We know that depends on , but we derive by so we can work with as constant. This implies:

This way we solved how first segments affects the rest of the equation in the derivation. Let’s revert the substitution:

This however is exactly the same as for derivation of the recursive functions by , so we can use same results:

Combined together, we get the final result for this formula:

### derive by ¶

This would be repetitive calculations as in derivation for , the key principles are same, we just go “deeper”.

### combined togehter¶

We are now able to combine previous results into the final equation:

We are now even able to write this calculation as generic formula, as the pattern is quite obvious here.

### implementation¶

Implementation wise, it is cruical to realize how the code works. In our case we want to load model of the robot at start of the application, and query it for solutions to IK for some set of angles.

This implies that are not known compile time, but are known when the application starts. Only thing that changes are the angles and desired point of end-effector.

In combined together, the final computation of derivation of is based on variables, out of which only can be computed at compile time.

Given that multiplication of quaternions is associative, we will define two new variables:

These represent the recursive rotations that occur in derivation of . That equation is now:

The following list represents order of operations to solve the equation calculated above:

- , , ,
- process 1./2. for all indexes in descending order
- process 3./4. for all indexes in ascending order
- For all indexes: