The rotation matrix

Turning a vector from one frame into the other, in two dimensions, by hand and in code.

U2.2Kinematics and framesUniversity30 min

Do this lesson in the simulator

A vector in the body frame and the same vector in the world frame differ by a rotation. In two dimensions that rotation is one matrix and two lines of code.

From body to world

A robot at heading h measuring (vx, vy) in its own frame is moving, in the world:

wx =  vx*cos(h) + vy*sin(h)
wy = -vx*sin(h) + vy*cos(h)

or, as a matrix,

[wx]   [ cos h   sin h ] [vx]
[wy] = [-sin h   cos h ] [vy]

That matrix is usually written R(h). The minus sign sits below the diagonal here because the heading is clockwise positive; in an anticlockwise convention it sits above. This is exactly the sort of detail that costs an afternoon, which is why the conventions were written out in the last lesson.

Checking it without trusting it

Substitute the cases you already know. At h = 0 the matrix is the identity: body and world agree, as they must, because heading 0 faces along +y and the body's forward is the world's y.

At h = 90 (facing along world +x), a body vector of (0, 10), ten centimetres straight ahead, gives wx = 10*sin(90) = 10 and wy = 10*cos(90) = 0. Ten centimetres along world x. Correct.

Two cases you can check by looking at the robot are worth more than a page of algebra you cannot.

In code

from bugbot import *
import math
connect()

def body_to_world(vx, vy, h_deg):
    a = math.radians(h_deg)
    return (vx * math.cos(a) + vy * math.sin(a),
            -vx * math.sin(a) + vy * math.cos(a))

for h in (0, 45, 90, 180, 270):
    print(h, [round(v, 2) for v in body_to_world(0, 10, h)])

Run this in the simulator

Note math.radians. Python's trigonometry works in radians and every angle in this API is in degrees. Forgetting the conversion gives you a robot that is wrong by a factor of 57, which is at least easy to spot.

The other way

The inverse of a rotation is a rotation the other way, so R(h) inverse is R(-h):

vx =  wx*cos(h) - wy*sin(h)
vy =  wx*sin(h) + wy*cos(h)

Two facts make this pleasant. A rotation matrix's inverse is its transpose, so no division is ever needed. And a rotation never changes a vector's length, so hypot(vx, vy) is the same in both frames. That last one is a useful check in code: if the length changed, the maths is wrong.

Task: rotate a vector

A robot at heading 60 measures (0, 10) in its own frame. Print the same vector in the world frame, as wx: and wy:, worked out with the rotation matrix.

from bugbot import *
import math
connect()

h = 60
vx, vy = 0.0, 10.0

Challenges

  1. Rotate a vector into the world and back again. Do you get the original to the last decimal place?
  2. Print the length of the vector in both frames and confirm it does not change.
  3. Write world_to_body() and check it against body_to_world() for ten random headings.