Not sure how to get this LsqFit working

Mervyn_Larrier · October 15, 2022, 7:28am

I have some data that I need to fit. All the other functions work, I’m just trying to minimize the cost function model in my code. Assuming you have all the same packages as me (the only errant one may be Underscores.jl), the notebook should run perfectly except for the calibrate code. The same error I get will be reproduced upon running the notebook. Thank you.

The errant script

using LinearAlgebra,Underscores, LsqFit
include("parallelIK.jl")
include("parallelJacobian.jl")
include("parallelFK.jl")
include("CostFunction.jl")

function podCalibrate()
    α,β = 40,85
    Rp,Rb = 300/2, 500/2
    
    s_nom1 = [92.1597,84.4488,0]
    s_nom2 = [27.055,122.037,0]
    s_nom3 = [-119.2146,37.5882,0]
    s_nom4 = [-199.2146,-37.5882,0]
    s_nom5 = [27.055,-122.037,0]
    s_nom6 = [92.1597,-84.4488,0]
    s_nom = [s_nom1 s_nom2 s_nom3 s_nom4 s_nom5 s_nom6]

    u_nom1 = [305.4001,111.1565,0]
    u_nom2 = [-56.4357,320.0625,0]
    u_nom3 = [-248.9644,208.9060,0]
    u_nom4 = [-248.9644,-208.9060,0]
    u_nom5 = [-56.4357,-320.0625,0]
    u_nom6 = [305.4001,-111.1565,0]

    u_nom = [u_nom1 u_nom2 u_nom3 u_nom4 u_nom5 u_nom6]

    l_nom = 604.8652*ones(1,6)
    Nominal = vcat(s_nom,u_nom,l_nom)
   
    
    sr = zeros(3,6)
    ur = zeros(3,6)

    sr[:,1] = [96.6610,81.7602,1.0684]
    sr[:,2] = [22.2476,125.2511,-0.5530]
    sr[:,3] = [-122.4519,36.6453,4.3547]
    sr[:,4] = [-120.6859,-34.4565,-4.9014]
    sr[:,5] = [24.7769,-125.0489,-4.8473]
    sr[:,6] = [91.3462,-80.9866,0.2515]

    ur[:,1] = [305.2599,115.0695,2.6210]
    ur[:,2] = [-55.2814,322.9819,4.2181]
    ur[:,3] = [-244.7954,208.0087,3.9365]
    ur[:,4] = [-252.5755,-211.8783,-3.0128]
    ur[:,5] = [-53.9678,-320.6115,4.3181]
    ur[:,6] = [302.4266,-109.4351,3.3812]

    lr = [604.4299 607.2473 600.441 605.9031 604.5251 600.0616]
    Real = vcat(sr,ur,lr)

    configs =[[-175.0000, 175.0000, 900.000, -1.2500, -0.0584, -0.0500],
    [175.0000,-175.0000, 575.4000, 0.0500, 0.0500,-0.0500],
    [170.0000, 172.0000, 555.4000, 0.0500,-1.5000, 2.2500],
    [-175.0000, 175.0000, 700.0000, -0.0500, 0.0500, -0.0500],
    [-174.0000,173.0000, 700.0000, 0.0500,-0.0500, 0.0500],
    [-175.0000, 175.0000, 570.3000, 0.0500, 0.0500, 3.0500],
    [170.0000, 175.0000, 561.7500, -0.0500, 0.0500, -0.0500],
    [-175.0000, -175.0000, 565.0000, -0.3500, -0.12500, -5.0500],
    [173.0000, -175.0000, 566.9000, 0.2500, -0.1500, -0.4500],
    [175.0000, -175.0000, 800.0000, -0.1500, 0.7500, 0.0500],
    [-171.0000, -175.0000, 565.4000, -0.0500, 0.2500, -1.2500]]
          
 
    #y is the vector of evaluated model solutions (F from Cost function)
    #x is the independent variables [the configurations?]
    #p model parameters? p0 would be Nominal
    configx = [configs[1] configs[2] configs[3] configs[4] configs[5] configs[6] configs[7] configs[8] configs[9] configs[10] configs[11]]
    nom_f = [Nominal[:,1],Nominal[:,2],Nominal[:,3],Nominal[:,4],Nominal[:,5],Nominal[:,6]]
    C = cost_func(configx,nom_f)
    fit = curve_fit(cost_func,configx,C,nom_f)

    return fit
end

The below script works

using LinearAlgebra,Underscores

include("parallelIK.jl")
include("parallelJacobian.jl")
include("parallelFK.jl")

function cost_func(configs::AbstractArray,params::AbstractArray)

    global configm = zeros(6,0) #for measured poses
    Rp,Rb = 300/2, 500/2
    α,β = 40,85
    d = size(configs)[2]

    for i =1:d #1:length(configs)
        @_ R,_,_,l,_,_ = parallelIK(configs[:,i], Rp, Rb, "xyz",α,β)
        B,_ = parallelFK(configs[:,i],l,Rp,Rb,"xyz",α,β,"true") #FK based on the true values
        global configm = hcat(configm,B)
    end
    
    loi = 604.8652*ones(6) #nominal leg lengths from table 1
    P = [0.0,0.0,700.0,0.0,0.0,0.0] #FK guess
    @_ Home,_ = parallelFK(P,loi,Rp,Rb,"xyz",α,β) #using the FK to get the home position
    @_ _,_,_,lo,_,_ = parallelIK(Home,Rp,Rb,"xyz",α,β) #nominal leg lenths at the home position

    F = zeros(1,0)
    lm = zeros(1,0)

    for j = 1:d #add another for loop so you can properly index through the legs
        for k = 1:6
            @_ _,_,_,l,_,_ = parallelIK(configs[:,j], Rp, Rb, "xyz",α,β)
            @_ Rm,_,_,_,_,_ = parallelIK(configm[:,j],Rp,Rb,"xyz",α,β) #could I use the outputs from here? no. doesn't make sense.

            lm = norm(configm[1:3,j]+Rm*params[k][1:3]-params[k][4:6])#measured length at current iteration
            C = (l[k]^2-((lo[k]+lm-params[k][7])^2))^2 #calculated leg lengths as well, push! to F
            F = hcat(F,C)
            
        end
    end
    return F
end

odow · October 15, 2022, 9:25am

Hi there!

Take a read of Please read: make it easier to help you.

You’re more likely to get help if you can provide a minimal working example that someone can copy paste and you provide the full text of the error. (Downloading and running a notebook, even if reproducible, is a lot of effort. Sometimes people can tell the problem without needing to run the code.)

Mervyn_Larrier · October 15, 2022, 7:20pm

Even with a lot of data? There’s a bunch of functions working together, so I provided the NB as I figured this would be easier since you can’t really tell anything from the code alone I don’t think. But I’ll update it.

odow · October 15, 2022, 7:41pm

What is the error? The key is to simplify your problem as much as possible so that it still produces the same error.

Mervyn_Larrier · October 15, 2022, 9:17pm

This is as simple as I know how to make it to reproduce this specific error.

MethodError: no method matching Array{Float64,1}(::Float64)
Closest candidates are:
  Array{Float64,1}() where T at boot.jl:425
  Array{Float64,1}(!Matched::UndefInitializer, !Matched::Int64) where T at boot.jl:406
  Array{Float64,1}(!Matched::UndefInitializer, !Matched::Int64...) where {T, N} at boot.jl:412
  ...

Stacktrace:
 [1] alloc_DF(::Array{Array{Float64,1},1}, ::Array{Float64,2}) at C:\Users\mervy\.julia\packages\NLSolversBase\cfJrN\src\objective_types\abstract.jl:19
 [2] lmfit(::LsqFit.var"#18#20"{typeof(cost_func),Array{Float64,2},Array{Float64,2}}, ::Array{Array{Float64,1},1}, ::Array{Float64,1}; autodiff::Symbol, kwargs::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at C:\Users\mervy\.julia\packages\LsqFit\hgZQe\src\curve_fit.jl:63
 [3] lmfit(::Function, ::Array{Array{Float64,1},1}, ::Array{Float64,1}) at C:\Users\mervy\.julia\packages\LsqFit\hgZQe\src\curve_fit.jl:61
 [4] curve_fit(::typeof(cost_func), ::Array{Float64,2}, ::Array{Float64,2}, ::Array{Array{Float64,1},1}; inplace::Bool, kwargs::Base.Iterators.Pairs{Union{},Union{},Tuple{},NamedTuple{(),Tuple{}}}) at C:\Users\mervy\.julia\packages\LsqFit\hgZQe\src\curve_fit.jl:115
 [5] curve_fit(::Function, ::Array{Float64,2}, ::Array{Float64,2}, ::Array{Array{Float64,1},1}) at C:\Users\mervy\.julia\packages\LsqFit\hgZQe\src\curve_fit.jl:106
 [6] podCalibrate() at c:\Users\mervy\Documents\RBE\Legged Robots\Omega\Midterm\Calibrate.jl:71
 [7] top-level scope at c:\Users\mervy\Documents\RBE\Legged Robots\Omega\Midterm\Untitled-1.ipynb:1

odow · October 15, 2022, 11:20pm

This doesn’t look right. I can reproduce the error with this:

using LsqFit
model(x, p) = p[1][1]*exp.(-x.*p[1][2])
xdata = collect(0:0.5:10)
xdata = reshape(xdata, length(xdata), 1)
ydata = model(xdata, [[1.0 2.0]]) + 0.01*randn(length(xdata))
p0 = [0.5, 0.5]
fit = curve_fit(model, xdata, ydata, [p0])

Your issues are:

xdata and ydata should be vectors, not matrices
p0 should be a vector of values, not a vector of vectors.

So something like this:

using LsqFit
model(x, p) = p[1]*exp.(-x.*p[2])
xdata = collect(0:0.5:10)
ydata = model(xdata, [1.0, 2.0]) + 0.01*randn(length(xdata))
p0 = [0.5, 0.5]
fit = curve_fit(model, xdata, ydata, p0)

Mervyn_Larrier · October 15, 2022, 11:32pm

So the issue it the data I’m passing. Is there any way that i can pass those values? The initial parameters are supoosed to be 42 kinematic parameters of s(3x1), u(3x1), l(1) per column. I tried a vector of vectors because that was the only way i could think to do it.

odow · October 15, 2022, 11:47pm

I’ve never used LsqFit before, so I’m just guessing from the documentation.

It looks like you might actually be able to pass matrices for xdata, but your ydata and p0 should still be vectors:

Is there any way that i can pass those values? The initial parameters are supoosed to b x 42 kinematic parameters of s(31), u(3 1), l(1) per column. I tried a vector of vectors because that was the only way i could think to do it.

Pass a vector that is b * 42 elements long.

odow · October 15, 2022, 11:50pm

I’ll also add that it’s a shame LsqFit is permissive by default. It lets you pass in types that might not work, and doesn’t provide appropriate error messages.

The JuMP style guide recommends against this, precisely because it can be hard to debug:
https://jump.dev/JuMP.jl/stable/developers/style/#Abstract-types-and-composition

x-ref No method matching curve_fit · Issue #215 · JuliaNLSolvers/LsqFit.jl · GitHub

Mervyn_Larrier · October 16, 2022, 12:53am

The documentation for it is so scant it’s frustrating.

Mervyn_Larrier · October 16, 2022, 12:57am

Sorry, I updated to clarify because I was on my phone and typos got in the way.
So for the parameters, instead of passing a vector containing vectors of the parameters I want to fit to, collapse it into a 42x1 column vector? something like

params = zeros(42,1)

?