Robot 10

Rotation Matrices? Why not, might be better. Let’s spike. Warning, this is gonna get a tiny bit mathematical.

We’ll take some things as given, because they’re pretty much provided as given in most of the math courses on the subject, though I think I could have derived these facts back when my math chops were fresh.

In particular, When it comes to transformations, rotations in particular, a vector is represented vertically:

And a rotation matrix is like this:

Suppose we have a matrix M

And a vector V:

M*v equals:

So we should be able to implement that easily enough, beginning with a test:

Matrices

        _:test("Multiply matrix times vector", function()
            local m = Matrix({1, 2, 
                             3, 4})
            local v = vec2(1,10)
            local w = m*v
            _:expect(w.x).is(21)
            _:expect(w.y).is(43)
        end)

Test should fail looking for Matrix:

1: Multiply matrix times vector -- TestMatrices:16: attempt to call a nil value (global 'Matrix')

Let’s code a bit:

Matrix = class()

function Matrix:init(array)
    self.m = array
    assert(#array==4, "must be 4 elements")
end

Should fail on *:

1: Multiply matrix times vector -- TestMatrices:19: bad argument #1 to 'mul' (`vec2' expected)

We can implement multiply this way:

function Matrix:__mul(v)
    local a,b,c,d = table.unpack(self.m)
    local x = v.x*a + v.y*b
    local y = v.x*c + v.y*d
    return vec2(x,y)
end

I expect the test to run. It does. So that’s nice, I can multiply a vector times a matrix and get a vector back. If the matrix is a rotation matrix, I’ll get the rotated vector back. Let’s do matrix multiply as well, with a new test.

        _:test("multiply two matrices", function()
            local m1 = Matrix{1,2,
                              3,4}
            local m2 = Matrix{3,5,
                              7,11}
            local m = m1*m2
            a,b,c,d = table.unpack(m.m)
            _:expect(a).is(17)
            _:expect(b).is(27)
            _:expect(c).is(37)
            _:expect(d).is(59)
        end)

My raw implementation for that is this:

function Matrix:__mul(v)
    if type(v)=="table" then
        local a,b,c,d = table.unpack(self.m)
        local w,x,y,z = table.unpack(v.m)
        local r1 = a*w+b*y
        local r2 = a*x+b*z
        local r3 = c*w+d*y
        local r4 = c*x+d*z
        return Matrix{r1,r2,r3,r4}
    end
    local a,b,c,d = table.unpack(self.m)
    local x = v.x*a + v.y*b
    local y = v.x*c + v.y*d
    return vec2(x,y)
end

We can clean this up a bit:

function Matrix:__mul(v)
    if self:isMatrix(v) then
        local a,b,c,d = table.unpack(self.m)
        local w,x,y,z = table.unpack(v.m)
        local r1 = a*w+b*y
        local r2 = a*x+b*z
        local r3 = c*w+d*y
        local r4 = c*x+d*z
        return Matrix{r1,r2,r3,r4}
    else -- must be vector
        local a,b,c,d = table.unpack(self.m)
        local x = v.x*a + v.y*b
        local y = v.x*c + v.y*d
        return vec2(x,y)
    end
end

function Matrix:isMatrix(m)
    if type(m) ~= "table" then return false end
    if m.is_a and m:is_a(Matrix) then return true end
    return false
end

That’s a bit more clear and the check for matrix is stronger. It’s difficult but not impossible to check whether it’s a vec2. It would require checking the metatable of the userdata item that is a vec2. We’ll skip that.

Oh. It comes to me how I might have expressed things in today’s earlier article. If the robot turns right, that’s the same as rotating all the data left. That’s why I had to use different swapping than I had expected.

Let’s go ahead here in matrix and do some rotations.

We’ll need two rotation matrices, which I’ll call rot90 and rot270. And we’ll access them this way:

        _:test("Rotate vectors", function()
            local v = vec2(2,3)
            local tr = Matrix:turnRight()
            local w = tr*v
            _:expect(w.x).is(-3)
            _:expect(w.y).is(2)
        end)

And implement turnRight … (I wonder if I’ll want to say turn("right")) … we’ll see. For now:

3: Rotate vectors -- TestMatrices:39: attempt to call a nil value (method 'turnRight')

And …

function Matrix:turnRight()
    return Matrix{0,-1,1,0}
end

Is the rotation matrix for 90 degrees, which is counterclockwise, which is just exactly the opposite of turning right. So the test should run. And it does.

Extend the test:

        _:test("Rotate vectors", function()
            local v = vec2(2,3)
            local tr = Matrix:turnRight()
            local w = tr*v
            _:expect(w.x).is(-3)
            _:expect(w.y).is(2)
            local tl = Matrix:turnLeft()
            w = tl*v
            _:expect(w.x).is(3)
            _:expect(w.y).is(-2)
        end)

And implement:

function Matrix:turnLeft()
    return Matrix{0,1,-1,0}
end

Tests all run. Houston, we have a matrix.

Let’s add one more matrix definition:

function Matrix:unit()
    return Matrix{1,0,0,1}
end

Now I think we can refactor our RotationLens to use Matrix. Let’s do.

Refactoring RotationLens

Is this a refactoring, or a rewrite? Well, it isn’t going to change behavior. But it is certainly replacing the algorithm. I don’t care what you call it. The tests will make sure that we don’t break anything.

RotatingLens works like this:

function RotatingLens:init(knowledge)
    self.knowledge = knowledge
    self.turns = 0
end

function RotatingLens:adjust(x,y)
    local xa,ya = x,y
    local turn = self.turns
    while turn > 0 do
        xa,ya = ya,-xa
        turn = turn - 1
    end
    return xa,ya
ens

function RotatingLens:turn(direction)
    if direction=="right" then
        self.turns = self.turns + 1
    elseif direction=="left" then
        self.turns = self.turns + 3
    end
    self.turns = self.turns%4
end

Let’s change it thus:

function RotatingLens:init(knowledge)
    self.knowledge = knowledge
    self.matrix = Matrix:unit()
end

function RotatingLens:adjust(x,y)
    local v = self.matrix*vec2(x,y)
    return v.x,v.y
end

function RotatingLens:turn(direction)
    if direction=="right" then
        self.matrix = self.matrix*Matrix:turnRight()
    elseif direction=="left" then
        self.matrix = self.matrix*Matrix:turnLeft()
    end
end

Test. That totally doesn’t work. I think I’ve got left and right wrong again, so I do this:

function RotatingLens:turn(direction)
    if direction=="right" then
        self.matrix = self.matrix*Matrix:turnLeft()
    elseif direction=="left" then
        self.matrix = self.matrix*Matrix:turnRight()
    end
end

If and when this works, I’ll sort it out. P.S. I really wish I had committed the Matrix. Let me do that now. Commit: Matrix operations.

Now the tests for rotation lens again.

Just one fails:

1: Right turn moves dead ahead to left turns -- 
Actual: nil, 
Expected: 1

That’s checking the turns variable, which no longer exists:

        _:test("Right turn moves dead ahead to left", function()
            local k = Knowledge()
            k:addFactAt("DA",0,1) -- dead ahead
            local lens = RotatingLens(k)
            _:expect(lens:factAt(0,1),"0 degrees").is("DA")
            lens:turn("right")
            _:expect(lens.turns,"turns").is(1)
            xa,ya = lens:adjust(-1,0)
            _:expect(xa,"xa").is(0)
            _:expect(ya,"ya").is(1)
            _:expect(lens:factAt(-1,0), "90").is("DA") -- off to left
        end)

Just remove that line and the tests should run. They do. Now I just hate that left turn right turn stuff. The big is that the Matrix methods should be called rotateRight and rotateLeft.

Let’s fix that:

function Matrix:rotateLeft()
    return Matrix{0,1,-1,0}
end

function Matrix:rotateRight()
    return Matrix{0,-1,1,0}
end

Now it makes sense to say this:

function RotatingLens:turn(direction)
    -- when robot turns right, world rotates left and vice versa
    if direction=="right" then
        self.matrix = self.matrix*Matrix:rotateLeft()
    elseif direction=="left" then
        self.matrix = self.matrix*Matrix:rotateRight()
    end
end

I think I still have a problem here. I think that if we want to do rotation R1 and then R2, the matrix should be R2*R1.

Do we even have a test for that, and could we tell the difference? I’ll begin by changing the code:

function RotatingLens:turn(direction)
    -- when robot turns right, world rotates left and vice versa
    if direction=="right" then
        self.matrix = Matrix:rotateLeft()*self.matrix
    elseif direction=="left" then
        self.matrix = Matrix:rotateRight()*self.matrix
    end
end

Curiously, I think we can never tell the difference, because it doesn’t matter which order we apply when the turns are all 90 degrees. Be that as it may, I think this is the formally correct version.

Let’s sum up here before I make more mistakes.

Summary

My tests are confirming my math and my intuition. When we turn left, the world turns right, and vice versa. And I hand-draw pictures and turned them around to get the values. For the matrix multiply, I typed in the formulas one way and did the numeric calculations another, and as you can see, with those numbers, there’s no real chance to get them wrong by choosing the wrong values to use.

Because the matrix only has ones and zeros in it, there is no chance of accumulated rounding errors in my matrix multiply: integers times and plus integers are integers. So we can just let the matrix accumulate. If trig didn’t work the way it does we might have to deal with drift, but we shouldn’t have that issue here.

Looking at where things stand, we have some gaps in our design. We’re not accumulating scan knowledge into our private version. We’re not really doing anything sensible about moving and turning together: when we turn, our motion should go, say, 1,0 instead of the usual 0,1. We can fix this by rotating the vector of motion … if we’re careful.

All this is too ad hoc. This is not inherently bad: we have been working to get a viable demo, and we are evolving the program and the design, indeed evolving our understanding of the problem and how to solve it. We should expect that our design will be rough … and we should be sure to improve it every chance we get.

What do I mean by chance? I mean every time we change the code we should improve it. Every time we read the code, if it’s not good enough, we should improve it. We need to keep the design quite close to our understanding. All our ideas should be in the code, not just in our heads.

Or so it seems to me. I said “should” a lot up there. I would do better to say something like “things go better for me when I”, and “I choose to focus on”, and such. You get to decide what works for you.

Anyway, now we have a Matrix, and we have a RotationLens that uses it. I think in the future, we’ll build the rotation directly into the regular Lens, but we’ll see about that.

Tomorrow is the last day before the walking skeleton will be shown to our stakeholders (me). I’d like to get moving and turning integrated, and I’d like to get our robot knowledge to accumulate everything it learns from all the scans, not just the most recent one.

But that’s for tomorrow. Today, I’ve got other matters to attend to.

See you next time!