I’ve been trying for over a week to translate this Matlab code into Julia. I’ve got the inverse kinematics, Jacobian, and other associate scripts translated. But this FK script is being a colossal pain. A link to videos of the code in action is available below. Any help would be very greatly appreciated! https://www.dropbox.com/scl/fo/o1y2ie4xpskb11v263rz7/h?dl=0&rlkey=9u348pww82ijn0cb6n90mbg45
My code for comparison to the MATLAB docs:
- Rotation Matrix
using LinearAlgebra
#= This function calculates the rotation matrix for the given angle and specified axis of rotation. The input
should be some angle in degrees. r,p,y coordinates should be input as u,v,w or equivalently x,y,z.
=#
function Rot(ψ::String,θ::Number)
θ = deg2rad(θ)
Rx = [1 0 0; 0 cos(θ) -sin(θ); 0 sin(θ) cos(θ)]
Ry = [cos(θ) 0 sin(θ); 0 1 0; -sin(θ) 0 cos(θ)]
Rz = [cos(θ) -sin(θ) 0; sin(θ) cos(θ) 0; 0 0 1]
m=
cmp(ψ,"x") == 1 ? Rx :
return Rx
cmp(ψ,"y") == 1 ? Ry :
return Ry
cmp(ψ,"z") == 1 ? Rz :
return Rz
#=otherwise=# Rz
end
IK
include("rotation.jl")
using LinearAlgebra
function parallelIK(P::Array{Float64}, Rp::Number, Rb::Number, γ::String, α::Number, β::Number=α)
O = P[1:3] #End effector origin
ϕ = P[4:6] #Euler Angles
α = deg2rad(α) #joint position difference angle for the platform
β = deg2rad(β) #joint position difference angle for the base
for h = 1:3
ϕ[h] = deg2rad(ϕ[h])
end
Rx = Rot("x",ϕ[1]); Ry = Rot("y",ϕ[2]); Rz = Rot("z", ϕ[3]);
Rots = cat(Rx*Ry*Rz,Rz*Ry*Rz,Rz*Ry*Rx,Rx*Ry*Rx,Rx*Rz*Rx,Ry*Rx*Ry,Ry*Rz*Ry,Rz*Rx*Rz, dims=3) #Matrix of rotation matrices
R = #=Acts like a switch statement to account for the various euler angles.=#
cmp(γ,"xyz") == 1 ? Rots[:,:,1] : #are these strings equal? If so, use this rotation matrix.
cmp(γ,"zyz") == 1 ? Rots[:,:,2] :
cmp(γ,"zyx") == 1 ? Rots[:,:,3] :
cmp(γ,"xyx") == 1 ? Rots[:,:,4] :
cmp(γ,"xzx") == 1 ? Rots[:,:,5] :
cmp(γ,"yxy") == 1 ? Rots[:,:,6] :
cmp(γ,"yzy") == 1 ? Rots[:,:,7] :
cmp(γ,"zxz") == 1 ? Rots[:,:,8] :
Rots[:,:,2]
s = zeros(3,6)
u = zeros(3,6)
L = zeros(3,6)
s[:,1] = [Rp*cos(β/2), Rp*sin(β/2), 0]
s[:,2] = [-Rp*sin(π/6-β/2), Rp*cos(π/6-β/2), 0]
s[:,3] = [-Rp*sin(π/6+β/2), Rp*cos(π/6+β/2),0]
s[:,4] = [-Rp*cos(π/3-β/2), -Rp*sin(π/3-β/2),0]
s[:,5] = [-Rp*cos(π/3+β/2), -Rp*sin(π/3+β/2),0]
s[:,6] = [Rp*cos(β/2), -Rp*sin(β/2), 0]
u[:,1] = [Rb*cos(α/2), Rb*sin(α/2),0]
u[:,2] = [-Rb*sin(π/6-α/2), Rb*cos(π/6-α/2),0]
u[:,3] = [-Rb*sin(π/6+α/2), Rb*cos(π/6+α/2),0]
u[:,4] = [-Rb*cos(π/3-α/2), -Rb*sin(π/3-α/2),0]
u[:,5] = [-Rb*cos(π/3+α/2), -Rb*sin(π/3+α/2),0]
u[:,6] = [Rb*cos(α/2), -Rb*sin(α/2), 0]
for i = 1:6
L[:,i] = O + R*s[:,i] - u[:,i] #Matrix of leg length vectors
end
return R, s, u, L
end
Jacobian of a parallel manipulator
using LinearAlgebra
function parallelJv(R::Matrix, s::Matrix, L::Matrix)
J = zeros(6,6)
Rs = zeros(3,6)
n = zeros(3,6)
l = zeros(6,1)
for i = 1:6
l[i] = norm(L[:,i]) #leg lengths
Rs[:,i] = R*s[:,i] #oriented joint position vector
n[:,i] = L[:,i]/l[i] #unit vector for each leg
J[i,:] = [n[:,i]' cross(Rs[:,i],n[:,i])'] #Jacobian calculated per leg
end
return l, n, J
end
The broken FK script:
include("rotation.jl")
include("parallelIK.jl")
include("parallelJacobian.jl")
function FK(P0::Vector, lg::Vector)
global Rm = 250.0/2 #platform radius
global Rf = 650.0/2 #base radius
global α = deg2rad(40) #angular position of base joints
global β = deg2rad(85) #angular position of platform joints
global ΔP = 1 #default positional error
global ϵ = 0.0001 #tolerable error
global Pos = zeros(6,1)
Pos[:,1] = P0; #given position, step 1
global i = 2
while ΔP > ϵ
ϕ = Pos[4:6,i-1] #Euler angles
a = deg2rad(ϕ[1])
b = deg2rad(ϕ[2])
c = deg2rad(ϕ[3])
B = [0 -sin(a) sin(b)*cos(a);0 cos(a) sin(b)*sin(a); 1 0 cos(b)]
T = [I(3) zeros(3,3); zeros(3,3) B]
global R,s,u,L = parallelIK(Pos[:,i-1], Rm, Rf, "zyz", α, β)
l,n,J = parallelJv(R,s,L)
Δl = lg-l #difference between given leg lengths and calculated leg lengths
D = Pos[:,i-1] + pinv(J*T) *Δl #calculating the next position
global Pos = hcat(Pos,D)
ΔL = norm(Δl) #magnitude of difference between calculated l and the given l (lg)
global ΔP = norm(Pos[:,i]-Pos[:,i-1])
println((ΔP,ΔL))
global i += 1
if i ==1000
break
end
end
return Pos[:,end]
end