From 0bf4f0d6f67069590bf2ccd391cd8cd6dfd00a2c Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 17:19:43 -0500 Subject: [PATCH 1/6] new type for a second amplitude --- flapopt/Wing2DOF.jl | 2 +- flapopt/run_w2d.jl | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/flapopt/Wing2DOF.jl b/flapopt/Wing2DOF.jl index 7040cef98..c3f90d404 100644 --- a/flapopt/Wing2DOF.jl +++ b/flapopt/Wing2DOF.jl @@ -45,7 +45,7 @@ NOTE: r2h::Float64 = 0.551 # r2hat second moment -- see [Whitney (2010)]. Default from 0.929 * 0.49^0.732 SEA::Bool = false # series-elastic https://github.com/avikde/robobee3d/pull/126 kSEA::Float64 = 1000 # SEA spring const - Amp::AbstractArray = [0.0,0.0] # Stroke, hinge amplitude for the desired output kinematics (set to 0 to not use) + Amp::AbstractArray = [0.0,0.0,0.0] # Stroke1, hinge, stroke2 amplitude for the desired output kinematics (set to 0 to not use) end # Fixed params ----------------- diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl index 866ed0e2a..08978c9d4 100644 --- a/flapopt/run_w2d.jl +++ b/flapopt/run_w2d.jl @@ -19,7 +19,7 @@ m = Wing2DOFModel( ko = 30.0, ma = 6, ka = 240, - Amp = deg2rad.([120, 140])) + Amp = deg2rad.([120, 140, 0])) ny, nu = cu.dims(m) function getInitialParams() @@ -40,7 +40,7 @@ include("w2d_paramopt.jl") # IMPORTANT - load which traj here!!! KINTYPE = 1 -N, trajt, traj0, opt, avgLift0 = initTraj(m, param0, KINTYPE; uampl=uampl) +N, trajt, traj0, opt, avgLift0 = initTraj(m, param0, KINTYPE; uampl=uampl, makeplot=true) # openLoopPlot(m, opt, param0) # Param opt init From 49181996a482ef6a03547de5ebe9a9c93c4a2300 Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 17:34:06 -0500 Subject: [PATCH 2/6] this creates a double traj --- flapopt/run_w2d.jl | 2 +- flapopt/w2d_paramopt.jl | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl index 08978c9d4..aef0fd9ca 100644 --- a/flapopt/run_w2d.jl +++ b/flapopt/run_w2d.jl @@ -19,7 +19,7 @@ m = Wing2DOFModel( ko = 30.0, ma = 6, ka = 240, - Amp = deg2rad.([120, 140, 0])) + Amp = deg2rad.([120, 140, 120])) ny, nu = cu.dims(m) function getInitialParams() diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl index 9cf8ff6a4..4447f7f16 100644 --- a/flapopt/w2d_paramopt.jl +++ b/flapopt/w2d_paramopt.jl @@ -46,8 +46,10 @@ fix -- Make traj satisfy dyn constraint with these params? """ function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=214, verbose=true, freq=0.165, N=80) if kinType==1 - opt = cu.OptOptions(false, false, 1/(N*freq), 1, :none, 1e-8, false) # sim - N = opt.boundaryConstraint == :symmetric ? N÷2 : N + Ncyc = (m.Amp[3]!=0.0 ? 2 : 1) + initialdt = 1/(N*freq) + opt = cu.OptOptions(false, false, initialdt, 1, :none, 1e-8, false) # sim + N = (opt.boundaryConstraint == :symmetric ? N÷2 : N)*Ncyc trajt, traj0 = createInitialTraj(m, opt, N, freq, [1e3, 1e2], param0, starti; uampl=uampl, verbose=verbose) elseif kinType==0 # Load data From e1fc9aa1e3e69591627be68860a78eebec0334eb Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 18:02:52 -0500 Subject: [PATCH 3/6] two stroke amplitudes in the traj --- flapopt/run_w2d.jl | 2 +- flapopt/w2d_paramopt.jl | 20 +++++++++++++++----- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl index aef0fd9ca..061cbcfb9 100644 --- a/flapopt/run_w2d.jl +++ b/flapopt/run_w2d.jl @@ -19,7 +19,7 @@ m = Wing2DOFModel( ko = 30.0, ma = 6, ka = 240, - Amp = deg2rad.([120, 140, 120])) + Amp = deg2rad.([120, 140, 60])) ny, nu = cu.dims(m) function getInitialParams() diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl index 4447f7f16..949b809ab 100644 --- a/flapopt/w2d_paramopt.jl +++ b/flapopt/w2d_paramopt.jl @@ -44,9 +44,10 @@ end kinType -- 0 => ID'ed real data, 1 => openloop sim with param0 then truncate, 2 => generate kinematics(t) fix -- Make traj satisfy dyn constraint with these params? """ -function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=214, verbose=true, freq=0.165, N=80) +function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=1073, verbose=true, freq=0.165, N=80) + Ncyc = (m.Amp[3]!=0.0 ? 2 : 1) + N *= Ncyc if kinType==1 - Ncyc = (m.Amp[3]!=0.0 ? 2 : 1) initialdt = 1/(N*freq) opt = cu.OptOptions(false, false, initialdt, 1, :none, 1e-8, false) # sim N = (opt.boundaryConstraint == :symmetric ? N÷2 : N)*Ncyc @@ -62,12 +63,21 @@ function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, ua error("Not implemented") end - @views tcoord(i) = traj0[i:ny:(N+1)*ny] + @views tcoord(i, Nn=N) = traj0[i:ny:(Nn+1)*ny] currentAmpl(i) = maximum(tcoord(i)) - minimum(tcoord(i)) for i=1:2 if m.Amp[i] != 0.0 # Set stroke/hinge amplitude https://github.com/avikde/robobee3d/pull/127 - tcoord(i) .*= m.Amp[i]/currentAmpl(i) # scale pos - tcoord(i+2) .*= m.Amp[i]/currentAmpl(i) # scale vel + iampl = currentAmpl(i) + if i == 1 && Ncyc > 1 + # https://github.com/avikde/robobee3d/pull/129 scale all by the second ampl then the first half + tcoord(i) .*= m.Amp[3]/iampl + tcoord(i+2) .*= m.Amp[3]/iampl + tcoord(i, N÷2) .*= m.Amp[1]/m.Amp[3] + tcoord(i+2, N÷2) .*= m.Amp[1]/m.Amp[3] + else + tcoord(i) .*= m.Amp[i]/iampl # scale pos + tcoord(i+2) .*= m.Amp[i]/iampl # scale vel + end end end From f275a67b4db34bc22128998cd06c075d4138e285 Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 18:24:41 -0500 Subject: [PATCH 4/6] get filtered traj --- flapopt/OptParams.jl | 6 +++--- flapopt/w2d_paramopt.jl | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/flapopt/OptParams.jl b/flapopt/OptParams.jl index 323cca962..ddcb25629 100644 --- a/flapopt/OptParams.jl +++ b/flapopt/OptParams.jl @@ -47,11 +47,11 @@ end "Some optimization numerical error is causing spiky oscillations in Δy https://github.com/avikde/robobee3d/pull/127#issuecomment-580050283" -function smoothΔy(Δy, ny, N, dt; ord=2, cutoff_freq=0.5) +function smoothTraj(traj, ny, N, dt; ord=2, cutoff_freq=0.5) # For each coord, fit a spline # tvec = collect(0:(N))*dt myfilter = digitalfilter(Lowpass(cutoff_freq; fs=1/dt), Butterworth(ord)) - smoothCoord(i) = filtfilt(myfilter, Δy[i:ny:(N+1)*ny]) + smoothCoord(i) = filtfilt(myfilter, traj[i:ny:(N+1)*ny]) trajNny = hcat([smoothCoord(i) for i=1:ny]...) return trajNny'[:] end @@ -63,7 +63,7 @@ function reconstructTrajFromΔy(m::Model, opt::OptOptions, POPTS, traj::Abstract np = length(pnew) dtold = paramold[end] # dt is the last param dtnew = pnew[end] - Δy = smoothΔy(Δy, ny, N, dtnew) + Δy = smoothTraj(Δy, ny, N, dtnew) Δyk = k -> Δy[(k-1)*ny+1 : k*ny] # Also convert the output traj with the Δy, new T, and inputs diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl index 949b809ab..7fc996faf 100644 --- a/flapopt/w2d_paramopt.jl +++ b/flapopt/w2d_paramopt.jl @@ -44,7 +44,7 @@ end kinType -- 0 => ID'ed real data, 1 => openloop sim with param0 then truncate, 2 => generate kinematics(t) fix -- Make traj satisfy dyn constraint with these params? """ -function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=1073, verbose=true, freq=0.165, N=80) +function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=1115, verbose=true, freq=0.165, N=80) Ncyc = (m.Amp[3]!=0.0 ? 2 : 1) N *= Ncyc if kinType==1 @@ -81,6 +81,11 @@ function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, ua end end + if Ncyc > 1 && m.Amp[1] != m.Amp[3] + # need to smooth the traj + traj0[1:(N+1)*ny] = cu.smoothTraj(traj0, ny, N, initialdt; ord=2, cutoff_freq=0.5) + end + if fix traj0 = cu.fixTrajWithDynConst(m, opt, traj0, param0) end From 49e9854696df0d6eaa42b9553a547820585f62ef Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 18:46:14 -0500 Subject: [PATCH 5/6] made opt be able to set --- flapopt/run_w2d.jl | 6 +++--- flapopt/w2d_paramopt.jl | 13 +++++++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl index 061cbcfb9..e60601c36 100644 --- a/flapopt/run_w2d.jl +++ b/flapopt/run_w2d.jl @@ -19,7 +19,7 @@ m = Wing2DOFModel( ko = 30.0, ma = 6, ka = 240, - Amp = deg2rad.([120, 140, 60])) + Amp = deg2rad.([120, 140, 0])) ny, nu = cu.dims(m) function getInitialParams() @@ -40,7 +40,7 @@ include("w2d_paramopt.jl") # IMPORTANT - load which traj here!!! KINTYPE = 1 -N, trajt, traj0, opt, avgLift0 = initTraj(m, param0, KINTYPE; uampl=uampl, makeplot=true) +N, trajt, traj0, opt, avgLift0 = initTraj(m, param0, KINTYPE; uampl=uampl) # openLoopPlot(m, opt, param0) # Param opt init @@ -253,7 +253,7 @@ end ret1 = KINTYPE==1 ? Dict("traj"=>traj0, "param"=>param0) : opt1(m, traj0, param0, 2, 0.1, 0.0) # In ID force tau2=0 # 2. Try to optimize -ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 3.0; Φ=120)#; print_level=3, max_iter=10000) +ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 1.9; Φ=[90,90])#; print_level=3, max_iter=10000) # testManyShifts(ret1, [0], 0.6) diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl index 7fc996faf..b58417387 100644 --- a/flapopt/w2d_paramopt.jl +++ b/flapopt/w2d_paramopt.jl @@ -164,7 +164,7 @@ end """One-off ID or opt""" function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, testAfter=false, testReconstruction=false, max_iter=4000, print_level=1, wARconstraintLinCbar=3.2, Φ=nothing) # A polytope constraint for the params: cbar >= cbarmin => -cbar <= -cbarmin. Second, τ2 <= 2*τ1 => -2*τ1 + τ2 <= 0 - print(mode==2 ? "ID" : "Opt", " Φ=", isnothing(Φ) ? "-" : Φ, ", minal=", minal, ", τ2/1 lim=", τ21ratiolim, " => ") + print(mode==2 ? "ID" : "Opt", " Φ=", isnothing(Φ) ? "-" : round.(Φ, digits=1), ", minal=", minal, ", τ2/1 lim=", τ21ratiolim, " => ") # cbar, τ1, mwing, wΨ, τ2, Aw, dt = param # Poly constraint @@ -180,11 +180,16 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t dp = [mlb; 0; 0; 0; wARb] if !isnothing(Φ) - m.Amp[1] = deg2rad(Φ) + if length(Φ) == 1 + m.Amp[1] = deg2rad(Φ) + else + m.Amp[1] = deg2rad(Φ[1]) + m.Amp[3] = deg2rad(Φ[2]) + end # get new input traj traj = initTraj(m, param, KINTYPE; uampl=75, verbose=false)[3] end - + Ncyc = (m.Amp[3]!=0.0 ? 2 : 1) ret = cu.optAffine(m, opt, traj, param, POPTS, mode, σamax; test=testAffine, Cp=Cp, dp=dp, print_level=print_level, max_iter=max_iter, testTrajReconstruction=testReconstruction) # append unactErr ret["unactErr"] = ret["eval_g"](ret["x"])[1:N] # 1 unact DOF @@ -201,7 +206,7 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t ret["FD∞"] = norm(ret["comps"][6][1,:], Inf) # Also store drag (should be same as uinf for scaling but dynamics) println(ret["status"], ", ", round.(ret["param"]', digits=3), - ", fHz=", round(1000/(N*ret["param"][end]), digits=1), + ", fHz=", round(Ncyc*1000/(N*ret["param"][end]), digits=1), ", al[mg]=", round(ret["al"] * 1000/9.81, digits=1), ", u∞=", round(ret["u∞"], digits=1), ", pow=", round(mean(abs.(ret["mechPow"])), digits=1), From 01abdfcad5775486ea3e5d2e0faf3e65ea8f14c8 Mon Sep 17 00:00:00 2001 From: Avik De Date: Thu, 30 Jan 2020 19:10:09 -0500 Subject: [PATCH 6/6] not sure this is working --- flapopt/run_w2d.jl | 2 +- flapopt/w2d_paramopt.jl | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl index e60601c36..977a60556 100644 --- a/flapopt/run_w2d.jl +++ b/flapopt/run_w2d.jl @@ -253,7 +253,7 @@ end ret1 = KINTYPE==1 ? Dict("traj"=>traj0, "param"=>param0) : opt1(m, traj0, param0, 2, 0.1, 0.0) # In ID force tau2=0 # 2. Try to optimize -ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 1.9; Φ=[90,90])#; print_level=3, max_iter=10000) +ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 1.9)#; Φ=[90,90])#; print_level=3, max_iter=10000) # testManyShifts(ret1, [0], 0.6) diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl index b58417387..4dd8f7735 100644 --- a/flapopt/w2d_paramopt.jl +++ b/flapopt/w2d_paramopt.jl @@ -83,7 +83,7 @@ function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, ua if Ncyc > 1 && m.Amp[1] != m.Amp[3] # need to smooth the traj - traj0[1:(N+1)*ny] = cu.smoothTraj(traj0, ny, N, initialdt; ord=2, cutoff_freq=0.5) + traj0[1:(N+1)*ny] = cu.smoothTraj(traj0, ny, N, initialdt; ord=2, cutoff_freq=1.5) end if fix @@ -182,6 +182,7 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t if !isnothing(Φ) if length(Φ) == 1 m.Amp[1] = deg2rad(Φ) + m.Amp[3] = 0.0 else m.Amp[1] = deg2rad(Φ[1]) m.Amp[3] = deg2rad(Φ[2]) @@ -206,7 +207,7 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t ret["FD∞"] = norm(ret["comps"][6][1,:], Inf) # Also store drag (should be same as uinf for scaling but dynamics) println(ret["status"], ", ", round.(ret["param"]', digits=3), - ", fHz=", round(Ncyc*1000/(N*ret["param"][end]), digits=1), + ", fHz=", round(1000/(Ncyc*N*ret["param"][end]), digits=1), ", al[mg]=", round(ret["al"] * 1000/9.81, digits=1), ", u∞=", round(ret["u∞"], digits=1), ", pow=", round(mean(abs.(ret["mechPow"])), digits=1),