# Kinematics - Maths¶

This page collects the various bits of vector maths used by Triangula’s chassis kinematics.

Hint

This document contains derivations for the equations used by the chassis code. If you don’t want to work through these you don’t need to, just look for boxes like this one which contain the most important bits. You don’t have to follow the maths here to use the code, but I’ve written it up anyway in the hope that it’ll be of interest.

## Motion at a point¶

Given an overall triangula.chassis.Motion, what is the velocity of a given point on the robot’s chassis? Calculating the velocity at each individual wheel is the first thing we need to do when working out how fast each wheel must be rotated.

Hint

Suppose we have a motion $$\vec{M}=\begin{pmatrix}m_x\\m_y\\m_\theta\end{pmatrix}$$ relative to the robot’s centre.

$$\theta$$ in the above expression is rotation in radians per second, where positive values correspond to clockwise motion when viewed from above.

We wish to determine the velocity $$\vec{V}=\begin{pmatrix}d_x\\d_y\end{pmatrix}$$ for a wheel $$W$$

The wheel is located, relative to the robot’s centre point, at location $$W=\begin{pmatrix}w_x&w_y\end{pmatrix}$$.

As the robot is a rigid structure, when the motion is purely a translation (i.e. $$m_\theta=0$$) all points on the robot will have the same velocity. Further, as we know that rotation and translation are independent, even when the rotation part is non-zero we can consider the two parts of the motion (rotation and translation) independently, just adding on the translation vector at the end. So, all we really need to work on is the rotation.

At this point we could do a lot of relatively awkward trigonometry, but there’s a simpler approach:

### Speed¶

We know how fast we’re moving, because we know the number of radians per second and we know the radius of the circle in which we’re moving. As we know the circumference of the circle is $$2\times\pi\times r$$, and we know that there are $$2\times\pi$$ radians (remember we use radians as our angular measurement!) in a circle, we can calculate we’re moving at $$2\pi r\times\frac\theta{2\pi}=r\theta$$ where $$r$$ is the radius of the circle and $$\theta$$ is the angular speed in radians per second.

We know $$\theta$$ directly as it’s part of our motion vector $$\vec{M}$$.

We can calculate $$r$$ because we know our wheel is at $$W$$ relative to our centre of rotation. We need what’s known as the magnitude, or length, of the vector from $$\begin{pmatrix}0&0\end{pmatrix}$$ to $$W$$, and basic geometry tells us that this quantity $$\left|W\right|=\sqrt{w_x^2+w_y^2}$$

Hint

Putting these together gives us the equation for the speed (note, not velocity, we haven’t worked out direction yet!) at this particular wheel to be:

$s=m_\theta\times\sqrt{w_x^2+w_y^2}$

### Direction¶

We know what direction we’re moving in. This is because we know where the centre of rotation is, and it’s always the case that when rotating around a point, the direction we move is perpendicular to the direction to the centre of rotation. If this isn’t immediately obvious, imagine there’s a rigid rod attached to the ground at one end and you’re holding the other end. As one end of the rod is attached to the ground you’re always going to move in a circle - you obviously can’t push in the direction the rod’s oriented as that would need you to change the length of the rod (you can’t, it’s rigid), so you can only move at right angles to that direction.

In the general case we can rotate a vector by multiplying it by a matrix, where the values in the matrix are functions of the angle through which we want to rotate (in this case positive values of $$\theta$$ correspond to clockwise rotation) - note that $$\theta$$ in this case is the angle through which we’re rotating the vector, and is not related to the $$m_\theta$$ part of the motion!

$\begin{split}\begin{bmatrix} x' \\ y' \\ \end{bmatrix} = \begin{bmatrix} \cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ \end{bmatrix}\end{split}$

In this particular case we want to rotate by a right angle to get the vector perpendicular to the radius of our circle and therefore parallel to its circumference. When $$\theta=\frac\pi2$$ all the values in the matrix above are either zero, one or minus 1:

$\begin{split}\begin{bmatrix} x' \\ y' \\ \end{bmatrix} = \begin{bmatrix} \cos \frac\pi2 & \sin \frac\pi2 \\ -\sin \frac\pi2 & \cos \frac\pi2 \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ -1 & 0 \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ \end{bmatrix} = \begin{bmatrix} y \\ -x \\ \end{bmatrix}\end{split}$

Hint

So, plugging $$W$$ into the above means our direction vector $$\vec{D}$$ is as follows:

$\begin{split}\vec{D} = \begin{pmatrix} w_y \\ -w_x \\ \end{pmatrix}\end{split}$

### Velocity from Rotation¶

As we have a direction and a speed we can calculate the velocity. First though we need to calculate the unit vector for the direction - this will give us a vector of magnitude 1, which we can simply multiply by our speed to get our wheel velocity. The unit vector is calculated by dividing each part of the direction vector by the magnitude of the vector, so:

$\widehat D=\frac{\overrightarrow D}{\left|D\right|}$

We know that the magnitude of a vector is the square root of the sum of the squares of its components, so we can work out that the unit vector in this case is:

$\widehat D=\frac{\overrightarrow D}{\sqrt{w_y^2+(-w_x)^2}}=\frac{\overrightarrow D}{\sqrt{w_x^2+w_y^2}}$

To get our velocity we multiple the unit vector by the speed:

\begin{align} \vec{V_{wheelRotation}} &= \widehat D \times s \\ &= \frac{\overrightarrow D}{\sqrt{w_x^2+w_y^2}} \times s \\ &= \frac{\overrightarrow D}{\sqrt{w_x^2+w_y^2}} \times m_\theta\times\sqrt{w_x^2+w_y^2} \\ &= {\overrightarrow D}m_\theta \\ \end{align}

Now everything simplifies out! We’re left with our wheel velocity being our direction vector multiplied by our angular velocity in radians per second. To finish the job we drop in our definition for $$\overrightarrow D$$ to get:

Hint

The velocity due to the rotation component of the motion at wheel $$W$$ is:

$\begin{split}\vec{V_{wheelRotation}} &= \begin{pmatrix} w_y -w_x \end{pmatrix}m_\theta\end{split}$

### Final Velocity¶

Hint

All our above calculations have only included the velocity from the rotation component of the motion. To include the translation component is easy though - we just add it on! Our final equation for the velocity of the wheel (or, more precisely, the velocity of the point at which the wheel makes contact with the ground) is therefore:

$\begin{split}\vec{V_{wheel}} = \begin{pmatrix} w_y \\ -w_x \\ \end{pmatrix}m_\theta + \begin{pmatrix} m_x \\ m_y \\ \end{pmatrix}\end{split}$

## Wheel Speed for Velocity¶

Triangula uses omni-wheels. Once we know from the above maths exactly what velocity is needed at each wheel position for a given desired motion we need to calculate the wheel speed in radians per second for each wheel. This is then passed on to the motor controllers to drive the robot.

Hint

Fistly we need a way to define the wheels. As used above, each wheel is located relative to the centre of the robot with a position vector, $$\vec{W}$$.

In addition to its position, we need to know two more things.

1. We need to know in what direction the wheel is pointing.
2. We need to know how big the wheel is, because a large wheel will require a smaller number of rotations or radians per second to achieve the same speed across the ground.

We can model these pieces of information as a single wheel drive vector, $$\vec{WD}$$, representing the direction and distance a regular wheel would roll in a single revolution.

Triangula’s triangula.chassis.HoloChassis.OmniWheel class contains the necessary logic to store the drive vector and to calculate it from other information such as wheel radius and angle (this may be more convenient when you need to specify your wheels). The maths, however, works on the drive vector as it’s simpler to deal with.

As we are using omni-wheels, any wheel on Triangula’s chassis can move in any direction. We know this by observation, but mathematically we know that we can drive the wheel along its drive vector $$\vec{WD}$$, and that the wheel can also freely roll at right angles to this vector. We cannot control or measure the degree of movement at right angles to our drive vector, so we can safely disregard it. All we care about is motion in the direction of the drive vector, and we can obtain this by projecting the velocity onto the drive vector, using the formula:

$p=\frac{\overrightarrow{V_{wheel}}\cdot\overrightarrow{WD}}{\left|WD\right|}$

For those not familiar with vector maths, the expression $$\vec{A}\cdot\vec{B}$$ sums the products of each component of each vector. In other words:

$\begin{split}\overrightarrow A\cdot\overrightarrow B=\begin{pmatrix}a_x\\a_y\end{pmatrix} \cdot\begin{pmatrix}b_x\\b_y\end{pmatrix}=a_x\times b_x+a_y\times b_y\end{split}$

So what are we doing when we project one vector onto another one? We’re working in a two-dimensional plane, in which any point can be defined by two coordinates. Typically we use x and y coordinates, something you’ll have encountered hundreds of times before in grids, maps, chess boards etc. What we actually mean when we use these though is slightly more subtle - we can think of both x and y as vectors themselves, which, when added together in the appropriate quantities, can be used to reach any point on the plane. So, our $$\vec{x}$$ represents a single unit movement along the x axis, and the $$\vec{y}$$ the same distance along the y axis. Starting from the origin, we can express any point on the plane as a motion involving a certain amount of $$\vec{x}$$ and a certain amount of $$\vec{y}$$.

The projection operation can be read as how much of unit vectors $$\vec{x}$$ and $$\vec{y}$$ do we need to add together to get a particular vector $$\vec{V}$$ ? We project our target vector onto our basis vectors (those used to represent the coordinate system) and read off the projection, which we can then use as a coordinate in that basis vector’s axis. When done with our regular x and y axes the results are exactly what you’d expect, the projection of a vector $$\begin{pmatrix}V_x\\V_y\end{pmatrix}$$ onto $$\vec{x}$$ is $$V_x$$ and onto $$\vec{y}$$ is $$V_y$$.

Using vectors which correspond to the x and y axes is very convenient and easy to understand, but if all we want is a pair of vectors which can, between them, reach every point on the plane, we don’t actually have to use those particular ones. In fact, all that’s required is any pair of vectors that are not co-incident, that is to say one is not a multiple of the other one.

Now, we know that our wheels have to have a velocity given by $$\vec{V_{wheel}}$$, and we know we have a drive vector $$\vec{WD}$$ and another vector which we haven’t bothered naming which is non-coincident to the drive vector in which the wheels can slide. What we want to know is how far we have to move per second in the direction of the drive vector such that in combination with an unknown amount of movement orthogonal to this (the sliding vector) we end up with the target wheel velocity.

So, we know that we need $$p$$ multiples of $$\widehat{WD}$$ to move as defined by $$\vec{V_{wheel}}$$, where $$p$$ is defined as :

$p=\frac{\overrightarrow{V_{wheel}}\cdot\overrightarrow{WD}}{\left|WD\right|}$

Hint

Now we know we need to move $$p$$ units of distance, to get the wheel speed in revolutions per second we simply divide by the distance travelled per revolution. As we already defined the drive vector to be the translation vector for a single revolution of the wheel we divide by $${\left|WD\right|}$$ again, to give wheel speed $$s$$ (as revolutions per second) as :

$s=\frac{\overrightarrow{V_{wheel}}\cdot\overrightarrow{WD}}{\left|WD\right|^2}$

## Wheel Speed from Motion¶

Combining the two sections above we can calculate the necessary speed for any wheel on the chassis for any target motion for the robot as a whole.

Hint

Given a wheel, with location relative to the origin of the chassis specified by $$\vec{W}$$ and drive vector $$\vec{WD}$$, defined as the vector described by the wheel hub after one revolution of the wheel, and a target motion vector $$M$$ consisting of $$m_x$$ and $$m_y$$ linear velocities and angular velocity $$m_\theta$$, we can calculate the speed at which the wheel will need to be driven, in revolutions per second, as:

\begin{align} s & = \frac{(\begin{pmatrix} w_y \\ -w_x \\ \end{pmatrix}m_\theta + \begin{pmatrix} m_x \\ m_y \\ \end{pmatrix})\cdot\overrightarrow{WD}}{\left|WD\right|^2} \\ & \\ & = \frac{\begin{pmatrix}w_ym_\theta+m_x\\-w_xm_\theta+m_y\end{pmatrix} \cdot\begin{pmatrix}wd_x\\{\mathrm{wd}}_y\end{pmatrix}}{wd_x^2+wd_y^2} \\ & \\ & = \frac{w_ym_\theta wd_x+m_xwd_x-w_xm_\theta wd_y+m_ywd_y}{wd_x^2+wd_y^2} \\ & \\ & = \frac{m_xwd_x+m_ywd_y+m_\theta(w_ywd_x-w_xwd_y)}{wd_x^2+wd_y^2} \end{align}

The most striking thing about the above equation is that wheel speed is a linear function of the components of the motion vector. Unless the chassis changes over time, the coefficients of $$m_x$$, $$m_y$$ and $$m_\theta$$ are constant, and can be pre-computed. A seemingly complex problem is therefore extremely simple to actually implement.

Triangula’s code is actually somewhat more complex, largely because in the sections above we have assumed that we are always rotating around the origin of the robot’s coordinate system. This assumption simplifies the maths, and allows for the surprisingly simple expression above, but in reality we occasionally want to specify rotation around a different point. For example. if carrying some kind of gripper we might want to always rotate around the gripper. In these cases the effective geometry does change, as the vectors describing the wheel locations are in fact relative to the centre of rotation under consideration rather than always being locked to the origin. This isn’t, however, much of an extra complication and if you’ve understood everything to this point you should be able to understand how the code works! The only real difference is that the code doesn’t reduce the equations down quite as much before running them.

## Motion from Wheel Speeds¶

Everything up to this point has focused on calculating wheel speeds for a given motion, but it is possible to go in the other direction and to calculate motion from observed wheel speeds. Note that we can only do this because we have at least as many wheels as we have dimensions in the motion vector (3 in this case). Also note that if our chassis had more than 3 wheels we would never have a precise solution - in effect each wheel contributes an equation in a system of linear simultaneous equations, so when we’re solving for 3 unknowns and have 3 equations we’ll (almost) always have a single well-formed unique solution, but the moment we add in more equations, especially given our measurements will by definition contain errors, we are very unlikely to ever have a perfect match and must use numerical methods to find the best approximation. Triangula doesn’t have this problem as she has 3 wheels, but were you to use this document to build something with, say, 5 wheels you’d need to consider this issue.

Because we can arbitrarily define the centre point for our motion we can set it to the origin of the robot’s coordinate space for convenience. This in turn means we can use the simplest form of the equations above, and that we can pre-compute the coefficients for each wheel. In fact, the code does exactly this - these lines in the init function for triangula.chassis.HoloChassis.OmniWheel should look familiar if you’ve just read the maths in the previous sections:

self.co_x = self.vector.x / self.vector_magnitude_squared
self.co_y = self.vector.y / self.vector_magnitude_squared
self.co_theta = (self.vector.x * self.position.y -
self.vector.y * self.position.x) / self.vector_magnitude_squared


Now rather than using $$m_x$$, $$m_y$$ and $$m_\theta$$ to find a set of wheel speeds, we need to use a set of wheel speeds, one for each wheel to find $$m_x$$, $$m_y$$ and $$m_\theta$$.

To prevent things getting out of hand in terms of size let’s set up some new terms. For a wheel $$w_{n\;\in1,2,3...}$$ with speed $$s_n$$ we can pre-compute three coefficients.

Hint

\begin{align} x_n & = \frac{wd_x}{wd_x^2+wd_y^2} \\ & \\ y_n & = \frac{wd_y}{wd_x^2+wd_y^2} \\ & \\ \theta_n & = \frac{w_ywd_x-w_xwd_y}{wd_x^2+wd_y^2} \end{align}

This allows us to concisely state three (in this case) simultaneous linear equations:

\begin{align} s_1 & = x_1m_x+y_1m_y+\theta_1m_\theta \\ s_2 & = x_2m_x+y_2m_y+\theta_2m_\theta \\ s_3 & = x_3m_x+y_3m_y+\theta_3m_\theta \\ \end{align}

Hint

As with any system of such equations we can express this in the form of a matrix:

$\begin{split}\begin{bmatrix}x_1&y_1&\theta_1\\x_2&y_2&\theta_2\\x_3&y_3&\theta_3\end{bmatrix}\begin{bmatrix}m_x\\m_y\\m_\theta\end{bmatrix}=\begin{bmatrix}s_1\\s_2\\s_3\end{bmatrix}\end{split}$

This is then amenable to numeric solving, in Triangula’s case we use the NumPy library, which also includes functions to handle the case where we have more wheels than 3, although obviously in this particular instance we don’t need to worry (Triangula is smart and fast, but she’s thus far been incapable of spontaneously growing wheels).

## Pose change from Motion¶

Once we have a known triangula.chassis.Motion we can work out the change in our triangula.chassis.Pose assuming the motion remains constant for a known time. The first stage is to understand that a constant motion represents movement around a circle - this might not seem immediately obvious, but imagine what will happen if you walk forwards (or in fact in any direction) and, every step you take, you turn slightly. You will walk in a circle, the more you turn and the smaller your steps the smaller the circle, turning less each step and taking longer strides results in a larger circle.

If we know we’re moving in a circle, it’s easy to work out the change in our position - we just need to know two things:

1. How big is the circle?
2. Where is the centre of the circle?

If we can work out these two things we can rotate our current location around the centre of the circle to get our new location. The proportion of the circle we travel around, and therefore the angle we need to rotate by, is determined by the angular velocity component of the motion.

First we need to work out the radius of the circle. We know that $$circ=2\pi{}r$$. We also know, from $$m_\theta$$ that in one second we’ll travel around $$\frac{m_\theta}{2\pi}$$ of the circumference of the circle. Our total distance travelled in one second is therefore $$\frac{m_\theta}{2\pi}\times2\pi r=m_\theta r$$

We know the total distance travelled in one second from the linear portion of $$M$$, so we can make these equal and solve for $$r$$ as follows:

\begin{align} m_\theta r & =\left|\begin{pmatrix}m_x\\m_y\end{pmatrix}\right| \\ \\ m_\theta r & =\sqrt{m_x^2+m_y^2} \\ \\ r & =\frac{\sqrt{m_x^2+m_y^2}}{m_\theta} \end{align}

Now we need to find the centre of the circle. We can do this easily, because we know that when moving around a circle the vector from our location to the centre of the circle is at right angles to the direction of our motion. There’s one slight catch here though - if we’re turning to the right ($$m_\theta > 0$$) our centre point should be to our right, which we can get by rotating our motion vector 90 degrees clockwise. If, however, we’re turning to the left we need to have our centre point to our left as well, rotating our motion vector 90 degrees counter-clockwise. For complete correctness we also need to handle the case where $$m_\theta = 0$$), corresponding to motion in a straight line with no angular component.

We’re taking a slight shortcut here, in that $$r$$ can be negative. This is helpful later - the radius has the same sign as the rotation part of the motion.

Note

Be very careful here! Our motion is expressed in robot coordinates, but we need everything to be in world coordinates if we’re rotating a location around another point. So, before we use any of our motion vectors we need to rotate the entire motion by the inverse of the pose orientation.

In the remainder of this section, when we refer to components of the motion i.e. $$m_y$$ these refer to the transformed version of the motion, i.e the motion as observed in the world, not the motion from the perspective of the robot. In the code the first thing we do is to rotate the motion vector by the negative of the orientation part of the current pose.

So, to get the position of the centre of the circle about which we’re moving we need to multiply our right angle unit vector (whether clockwise or counterclockwise) by the radius, to get a vector in the same direction with length $$r$$.

\begin{align} \widehat R & =\left\{\begin{array}{l}m_\theta>0\;:\;\frac{\begin{pmatrix}m_y\\-m_x\end{pmatrix}}{\sqrt{m_x^2+m_y^2}}\\m_\theta<0\;:\;\frac{\begin{pmatrix}-m_y\\m_x\end{pmatrix}}{\sqrt{m_x^2+m_y^2}}\end{array}\right. \\ \\ \overrightarrow R=\widehat Rr & =\frac{\begin{pmatrix}m_y\\-m_x\end{pmatrix}}{m_\theta} \end{align}

So, when $$m_\theta$$ is non-zero we rotate our current location around the point obtained by adding $$\overrightarrow R$$ to that location by $$m_\theta$$ radians.

To rotate a point $$P$$ around a centre point $$C$$ clockwise by $$\theta$$ radians we apply the following transformation:

\begin{align} rotate(P,\;C,\;\theta)=\begin{bmatrix}p'_x\\p'_y\end{bmatrix}=\begin{bmatrix}p_x-c_x\\p_y-c_y\end{bmatrix}\begin{bmatrix}\cos(\theta)&\sin(\theta)\\-\sin(\theta)&\cos(\theta)\end{bmatrix}+\begin{bmatrix}c_x\\\;c_\mathrm y\end{bmatrix} \end{align}

Hint

To obtain the result of motion $$M$$ on pose $$P$$ after time $$t$$:

• If $$m_\theta$$ is non-zero the new pose $$P'$$ is found by rotating the current pose about the centre point $$\begin{pmatrix}p_x+r_x\\p_y+r_y\end{pmatrix}$$ as defined above, by $$m_\theta{}t$$ radians.
• If $$m_\theta$$ is zero the new pose $$P'$$ is found by adding the (transformed) linear part of the motion multiplied by $$t$$ to the position part of the original pose.

In Python, using a helpful vector library, the code is actually relatively simple:

# Total delta in orientation angle over the time interval
orientation_delta = motion.rotation * time_delta
# Scaled translation vector rotated into world coordinate space (motion uses robot space)
translation_vector_world = rotate_vector(motion.translation, self.orientation) * time_delta

if orientation_delta == 0:
# No orientation, trivially add the rotated, scaled, translation vector to the current pose
return self.translate(translation_vector_world)
else:
centre_of_rotation = self.position + translation_vector_world.cross() / orientation_delta
':type : euclid.Point2'
final_position = rotate_point(self.position, angle=orientation_delta, origin=centre_of_rotation)
return Pose(position=final_position, orientation=self.orientation + orientation_delta)