Kinematics - Maths¶

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.

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:

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.

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:

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:

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 :

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.

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.

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

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:

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.

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\).

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:

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)