# Below is function for angle reconstruction, using Euler Angles:
# (xyz (pitch-roll-yaw) convention)
# helper function: norm_vec
# FUNCTION 0
norm_vec <- function(x) sqrt(sum(x^2))
# FUNCTION 1: reconstruction of joint angle
recon <- function(theta1,psi1,phi1,theta2,psi2,phi2){
# rotations can be written as rotation matrices:
# A = BCD
# Source: https://mathworld.wolfram.com/EulerAngles.html
#theta is pitch, psi is roll, and phi is yaw
# Convert to radian
theta1=theta1/180*pi;
theta2=theta2/180*pi;
psi1=psi1/180*pi;
psi2=psi2/180*pi;
phi1=phi1/180*pi;
phi2=phi2/180*pi;
#Device 1
B1 = matrix(c(1,0,0,0,cos(psi1),-sin(psi1),0,sin(psi1),cos(psi1)),
nrow = 3,ncol = 3)
C1 = matrix(c(cos(theta1),0,sin(theta1),0,1,0,-sin(theta1),0,cos(theta1)),
nrow = 3,ncol = 3)
D1 = matrix(c(cos(phi1),-sin(phi1),0,sin(phi1),cos(phi1),0,0,0,1),
nrow = 3,ncol = 3)
#Device 2
B2 = matrix(c(1,0,0,0,cos(psi2),-sin(psi2),0,sin(psi2),cos(psi2)),
nrow = 3,ncol = 3)
C2 = matrix(c(cos(theta2),0,sin(theta2),0,1,0,-sin(theta2),0,cos(theta2)),
nrow = 3,ncol = 3)
D2 = matrix(c(cos(phi2),-sin(phi2),0,sin(phi2),cos(phi2),0,0,0,1),
nrow = 3,ncol = 3)
# Total Rotation Matrices:
A1 = B1%*%C1%*%D1;
A2 = B2%*%C2%*%D2;
# define a initial unit normal vector for each of the two sensors
vec1 = matrix(c(0,0,1),nrow=1);
vec2 = matrix(c(0,0,1),nrow=1);
# final orientation
fnl1 = vec1%*%A1;
fnl2 = vec2%*%A2;
# Calculation of the angle between the two normal vectors
cosangle = (fnl1%*%t(fnl2))/norm_vec(fnl1)/norm_vec(fnl2)
angle = acos(cosangle);
return (180-angle/pi*180)
}
# FUNCTION 2: vectorization and plot drawing
vec_plot = function(data1,data2,plottitle){
# Note: all the hyperparameters here are dataframes of the 2 devices
len = min(length(data1[,4]),length(data2[,4]));
# initialize a vector for storage
anglevec = matrix(0,nrow = len-1);
for (i in 2:len){
# implement the reconstruction algorithm
angle = recon(as.numeric(data1[i,4]),as.numeric(data1[i,5]),as.numeric(data1[i,6]),
as.numeric(data2[i,4]),as.numeric(data2[i,5]),as.numeric(data2[i,6]));
anglevec[i-1] = angle;
}
# plotting section
plot((as.numeric(data1[2:len,3])+as.numeric(data2[2:len,3]))/2,anglevec,type = “l”,xlab=’time(s)’,ylab=’angle(deg)’,main=plottitle)
# return rectified time and angle as a dataframe
dfres = as.data.frame(matrix(c((as.numeric(data1[2:len,3])+as.numeric(data2[2:len,3]))/2,anglevec),ncol = 2));
colnames(dfres) <- c(“time(s)”,”angle(deg)”)
return (dfres)
}
# do not run until successfully collect and import data.
# Implication code
angle_mech <- vec_plot(D1,D2,”real-time angle measurement example”)