DEV Community

Cover image for Learn Julia (13): Linear Algebra and Rotations
Z. QIU
Z. QIU

Posted on

3 1

Learn Julia (13): Linear Algebra and Rotations

The LinearAlgebra module of Julia brings many matrix-related functionalities to us. Thus I would like to revise some robotic knowledge while learning this module.

A presentation about 3 axes in space and the according rotations:
Alt Text

Alt Text

Now let's code the roll, pitch and yaw matrices:

using LinearAlgebra
## roll: about x-axis
function getRotx(theta::Float64, angleInDeg = false) 
    if (angleInDeg)
        theta = theta*pi/180.0
    end
    return [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)]
end

## pitch: about y-axis
function getRoty(theta::Float64, angleInDeg = false) 
    if (angleInDeg)
        theta = theta*pi/180.0
    end
    return [cos(theta) 0 sin(theta); 0 1 0;-sin(theta) 0 cos(theta)]
end

## yaw: about z-axis
function getRotz(theta::Float64, angleInDeg = false) 
    if (angleInDeg)
        theta = theta*pi/180.0
    end
    return [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0;0 0 1]
end
Enter fullscreen mode Exit fullscreen mode

Several important groups definition in mathematics:
Alt Text

The orthogonal group in dimension 3 is named SO(3) which is very largely used in Robotics. Any rotation matrix in 3D space belongs to this SO(3) group.

## check if a matrix belongs to group SO(3)
function isInSO3(R)
    res = ( size(R) == (3, 3) )
    if res 
        res = (transpose(R) * R == I)
    end
    res
end 
Enter fullscreen mode Exit fullscreen mode

Now define Rotation, Vec3, quaternion as well as their related functions.

# Identity matrix for 3D space
I33 = [1 0 0; 0 1 0; 0 0 1]


##  definition of a 3D vector Datatype 
##  for representing a vector in space 
struct Vec3
    v1::Float64
    v2::Float64
    v3::Float64
end

# converting a Vec3 to a 3x1 array
function Vec3toArray(v::Vec3)
    return [v.v1, v.v2, v.v3]
end

## A rotation in 3d space consists of an axis and an angle about the axis
struct Rot3
    unitAxis::Vec3
    thetaInRad::Float64
end

# Quaternion datatype
mutable struct Quaternion
    q0::Float64
    q1::Float64
    q2::Float64
    q3::Float64
end

# converting a 3d rotation to a Quaternion respresentation 
function getQuaternion(R::Rot3)
   q = Quaternion(1.0, 0.0, 0.0, 0.0)
   axis = Vec3toArray(R.unitAxis)
   if norm(axis) - 1.0 > 1e-10
       println("warning: not unit axis")
       axis = axis./norm(axis)
    end
    sin_theta = sin(0.5*R.thetaInRad)
    q.q0 = cos(0.5*R.thetaInRad)
    q.q1 = axis[1]*sin_theta
    q.q2 = axis[2]*sin_theta
    q.q3 = axis[3]*sin_theta
    return q
end

## convert a 3d vector to its skew-matrix form which belongs to group so(3)
function getSkew3(v::Vec3)
   return [  0      -v.v3   v.v2;
            v.v3    0       -v.v1;
           -v.v2    v.v1    0]

end

## converting a 3d rotation to a matrix respresentation 
function getRotMatrix(r::Rot3)
    q = getQuaternion(r)
    # omega = [q.q1, q.q1, q.q1]

    if r.thetaInRad == 0.0    #TODO: also include 2n*pi
        omega = Vec3(0.0, 0.0, 0.0)
    else
        alpha = sin( 0.5* r.thetaInRad )
        omega = Vec3(q.q1/alpha, q.q2/alpha, q.q3/alpha) 
    end
    sk = getSkew3(omega)
    println("omega:   $omega")
    println("skew: ", sk)
    return I33 .+ sin( r.thetaInRad )* sk .+ (1 - cos( r.thetaInRad ))* sk* sk
end

Enter fullscreen mode Exit fullscreen mode

Now test the functions defined above:

## rotate about z-axis for 0.3*pi
r1 = Rot3( Vec3(0.0,0.0,1.0), 0.3*pi)
Mrot1 = getRotMatrix(r1)

## rotate about x-axis for 0.2pi
r2 = Rot3( Vec3(1.0,0.0,0.0), 0.2*pi)
Mrot2 = getRotMatrix(r2)

## rotate about y-axis for 0.15pi
r3 = Rot3( Vec3(0.0,1.0,0.0), 0.15*pi)
Mrot3 = getRotMatrix(r3)

println("Mrot1: $Mrot1 ")

println("det(Mrot1): ", det(Mrot1))
println("trace(Mrot1): ", tr(Mrot1))
println("inv(Mrot1): ", inv(Mrot1))

## check if the results are correct
println(Mrot1 .- getRotz(0.3*pi))
println(Mrot2 .- getRotx(0.2*pi))
println(Mrot3 .- getRoty(0.15*pi))

Enter fullscreen mode Exit fullscreen mode

Alt Text

Now let's do a little visualization test. In the code below, points shall be rotated by 45 deg about z-axis:

points = [0 0 0.5; 1 0 0.5 ]

x = points[:,1]
y = points[:,2]
z = points[:,3]

r4 = Rot3( Vec3(0.0,0.0,1.0), 0.25*pi) # 45 degree about z-axis
Mrot4 = getRotMatrix(r4)

points_new = transpose(Mrot4* transpose(points))
xn = points_new[:,1]
yn = points_new[:,2]
zn = points_new[:,3]

plotting = true
if plotting
        using Plots
    plotlyjs()

    display(plot(x, y, z, xlabel = "x", ylabel = "y", zlabel ="z", w = 10))
    display(plot!(xn, yn, zn, w = 10))
    display(plot!(zeros(11), zeros(11), 0:0.1:1, w = 3)) 
    display(plot!(zeros(11), 0:0.1:1, zeros(11), w = 3) )
    display(plot!(0:0.1:1, zeros(11), zeros(11), w = 3) )
end

Enter fullscreen mode Exit fullscreen mode

I cut two snapshots from the displayed 3D chart, it can be seen that point [1 0 0.5] has been rotated by 45 degrees about z-axis :
Alt Text

Image of Timescale

🚀 pgai Vectorizer: SQLAlchemy and LiteLLM Make Vector Search Simple

We built pgai Vectorizer to simplify embedding management for AI applications—without needing a separate database or complex infrastructure. Since launch, developers have created over 3,000 vectorizers on Timescale Cloud, with many more self-hosted.

Read more →

Top comments (1)

Collapse
 
phpsmarter profile image
phpsmarter

thanks! very helpful!

Sentry image

See why 4M developers consider Sentry, “not bad.”

Fixing code doesn’t have to be the worst part of your day. Learn how Sentry can help.

Learn more

👋 Kindness is contagious

Explore a sea of insights with this enlightening post, highly esteemed within the nurturing DEV Community. Coders of all stripes are invited to participate and contribute to our shared knowledge.

Expressing gratitude with a simple "thank you" can make a big impact. Leave your thanks in the comments!

On DEV, exchanging ideas smooths our way and strengthens our community bonds. Found this useful? A quick note of thanks to the author can mean a lot.

Okay