Learn Julia (13): Linear Algebra and Rotations
Tags: linearalgebra, robotics, rotation, julia
Published: 2020-11-27 20:39:27

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](https://dev-to-uploads.s3.amazonaws.com/i/rq7mpb78cidwxdi0p4af.png)


 ![Alt Text](https://dev-to-uploads.s3.amazonaws.com/i/rfa7yleb39qox4wlv2t6.PNG)

Now let's code the `roll`, `pitch` and `yaw` matrices:
```julia
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
```

Several important **groups** definition in mathematics:
![Alt Text](https://dev-to-uploads.s3.amazonaws.com/i/mejfayturtaq4yksncen.png)

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.
```julia
## 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 
```

Now define **Rotation**, **Vec3**, **quaternion** as well as their related functions. 
```julia
# 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

```

Now test the functions defined above:
```julia
## 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))

```
![Alt Text](https://dev-to-uploads.s3.amazonaws.com/i/v57552gp821vlcs6e46b.PNG)

Now let's do a little visualization test. In the code below, `points` shall be rotated by 45 deg about z-axis:
```julia
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

```
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](https://dev-to-uploads.s3.amazonaws.com/i/xz4tc4744zj21dsy4jwg.png)