I need to convert these parallel robot algorithms to Julia from Matlab, but I cannot get the forward kinematics to work

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:

  1. 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

Can you share the Julia code you have so far? And maybe say where the trouble is occurring (if you know)? That would greatly help everyone offer advice/code.

1 Like

Some tips on posting problems to the forum. Best of luck!

2 Likes

I have updated the question to include my code. All the dependent functions are present as well.

2 Likes

How is the FK script broken, exactly? Wrong answers, errors thrown, does the problem happen when running the script or running the FK method? Can we assume that the rotation.jl, parallelIK.jl, and parallelJacobian.jl scripts were listed above but just not labeled as such?

1 Like

Yes, the labels for the functions should be directly above them. I’m sorry if they’re not showing up for you. You are correct in your assessment. As for what is wrong, the script I’ve written basically compounds the error. I’m trying to minimize the error. So I want the calculated position to match the position I’ve been given.
So no errors are thrown, but the difference between the end-effector position I was given and the end-effector position that’s been calculated only grows.

So an iterative method FK is failing to converge, that information helps.

Do you really not have the MATLAB source code? The screenshots aren’t named to indicate any sort of organization and order, and each one is only a segment of the code. It’s almost impossible to follow by itself, which is why the “Please read: make it easier to help you” post said " Don’t post your code as a screenshot ; it is difficult to read and impossible to copy-paste." I tried to use the Julia code to compare to the screenshots, but it’s also difficult because you don’t often use the same names or methods between the Julia code and MATLAB code. It’s not obvious how you are calling the FK method in Julia.

1 Like

Unfortunately, I truly do not have the MATLAB code. But you were right, it was incredibly shortsighted of me to not rename the files. I’ve updated the file names to reflect which scripts the screenshots are a part of.

Also, could you elaborate a bit about what you mean by the call being non-obvious? I’m not quite sure I follow you.

The Julia scripts don’t have any method call necessary to run any of the functions you defined. If say, FK had 0 arguments, it’d be obvious we just call FK(), but FK has 2 arguments.

It’s strange because the MATLAB version has function p = FK, so there are 0 arguments there. I see a local P0 assigned as an initial condition in one screenshot but commented out in the next screenshot, and I don’t see lg assigned anywhere, just used in step 4.

So you aren’t able to run your code yourself to check if the MATLAB version even works? Did someone else provide you the screenshots? Because editing between screenshots without informing you is the only reason I can imagine the P0 line being bizarrely commented out in between screenshots and the lg assignment being missing.

Whatever the case is, you really should have the MATLAB source code, even if you have to type it yourself from screenshots, to run and test while you’re translating it to another language. It’s just due diligence to match the code behavior between the versions and also to inform us what exact output it is supposed to have. It’s not enough to just say “I want the calculated position to match the position I’ve been given”, only code with hard numbers and printouts can tell us what’s going on.

1 Like

I do have some good news. The lecture videos I’m referencing have recently been made to be downloadable and they can explain this better than I. We needn’t bother with errant screenshots at all. Allow me a moment to update the dropbox.
I should also mention that the reason I wasn’t able to first replicate it in MATLAB is that I currently don’t have access. I’m working on rectifying that for future incidents like this.

Uploading a video isn’t enough (I’ll get back to this), write out the MATLAB source and test it yourself. You might need to write a test script yourself, the point is to have source files anybody can download and run to see firsthand the same results as you do. MATLAB not being free is an obstacle that may stop many people from helping, but it’s an expected pitfall of working with MATLAB code. After you run the MATLAB code and are certain it works, then try to translate the MATLAB source to Julia again, and this time really try to keep the same variable names so it’s easier for readers to compare.

I skimmed through the videos, the reason for the P0 and lg discrepancies is that the screenshots were taken from 2 different videos, and it’s obvious that the code was edited between the videos. FK was an unfinished 0-argument function by the end of the 1st video, but in the 2nd video, FK behaves as a 2-argument function with the first 12 lines never shown. Moreover, the presenter is scrolling around the code and editing it in real-time. This is just not a good way to share code.

2 Likes

I know you’re not concerned about speed yet, but it would be worth trying to get rid of all the global keywords since those could be very bad for performance. Most things could be passed as function arguments, and there are a bunch of globals that appear to just be constants. So you can pull those out of the functions and call them with const instead of global.

1 Like

Not sure if this is the cause (or if I’m missing something), but in the function FK, α and β are converted to radians with deg2rad. Then, they are passed directly into parrellIK where deg2rad is used again, so it appears that the result would be de2rad(deg2rad(α)), which seems like a mistake.

It also seems like it would be worthwhile to convert everything to radians up front, do all the calculations, and then convert the results back to degrees only at the end. This would 1) reduce the number of deg2rad calls and make the code look cleaner and easier to read, and 2) improve performance since you won’t be doing deg2rad repeatedly within the inner loops.

3 Likes

Thank you for that bit of advice. I’ll modify the code to reflect that.

1 Like

I spent some time trying to figure out the issue, and I am having (I think) the same issue where my end values are huge, even quitting after just 8 runs through the loop. One thing I don’t understand from the screenshots is that in the MATLAB IK function, the position array is set to P = [0 10 150 0 0 0] explicitly, and I can’t make sense of how to handle that since hardcoding it into IK would seem to make that function recalculate the same thing every time. Perhaps that is the issue.

2 Likes

Might you elaborate on that a bit more? I apologize for my absence, I was taking the time to acquire a copy of MATLAB to rewrite the script. Yes, my end values end up huge; e13 and up. Once I finish rewriting the MATLAB, I’ll post it.

1 Like

I have acquired access to MATLAB, rewritten the code, and uploaded it. It’s not perfect, but it’s returning reasonable positions that agree with the IK. Since the FK solution is non-unique, it has to be iterative. It didn’t converge to the position I gave it, but it does converge to a position that agrees with the inverse kinematics. Help is still very much desired.
EDIT: Checking with my Professor, as long as it agrees with the IK, that is acceptable. So the FK Matlab script I provided is sound.

2 Likes

Isn’t that intended? Iterative solutions start from initial conditions, they don’t try to approach it. From what I understood, the FK is supposed to agree with the IK, so this just sounds like a good thing. Going forward, people should use the .m files as a reference because they differ a lot from the screenshots of unfinished code.

Now you should do this step and test the Julia code the same way you tested the MATLAB code. I see that you haven’t uploaded the tests you did for MATLAB to the dropbox, so we still don’t know what inputs you’re giving the [Pose,Dp] = FK(P0,lg,a) function. It’s really important that you write the Julia code as similar as possible for comparison’s sake. There are a few obvious examples:

  1. translate alpha to alpha, not α; matching exact variable names is easier to compare. That includes the method names, so translate IK to IK, not parallelIK.
  2. As @mihalybaci mentions, your current Julia FK method assigns a lot of global variables, and that is clearly not done in the MATLAB code. Global variables live independently from the method, so this is a major behavior change, not just an aesthetic one.
  3. Avoid translating (pi/180)* to deg2rad(. As @mihalybaci mentioned upthread, this translation change can lead to typos where you do it twice.

Update the post with the new Julia code when you’re done. Include a test script and a report of the results, along with a comparison to test of the MATLAB you wrote. Best case scenario, the new Julia code agrees and you just get to show off your successful translation.

2 Likes

I will make sure to update accorrding to your specifications.