Skip to content

Maneuverability by having two stroke amplitudes in the traJ #129

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
avikde wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Maneuverability by having two stroke amplitudes in the traJ #129

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
0bf4f0d
new type for a second amplitude
avikde Jan 30, 2020
4918199
this creates a double traj
avikde Jan 30, 2020
e1fc9aa
two stroke amplitudes in the traj
avikde Jan 30, 2020
f275a67
get filtered traj
avikde Jan 30, 2020
49e9854
made opt be able to set
avikde Jan 30, 2020
01abdfc
not sure this is working
avikde Jan 31, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions flapopt/OptParams.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion flapopt/Wing2DOF.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 -----------------
Expand Down
4 changes: 2 additions & 2 deletions flapopt/run_w2d.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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)

Expand Down
43 changes: 33 additions & 10 deletions flapopt/w2d_paramopt.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,13 @@ 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=1115, verbose=true, freq=0.165, N=80)
Ncyc = (m.Amp[3]!=0.0 ? 2 : 1)
N *= Ncyc
if kinType==1
opt = cu.OptOptions(false, false, 1/(N*freq), 1, :none, 1e-8, false) # sim
N = opt.boundaryConstraint == :symmetric ? N÷2 : N
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
Expand All @@ -60,15 +63,29 @@ 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

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=1.5)
end

if fix
traj0 = cu.fixTrajWithDynConst(m, opt, traj0, param0)
end
Expand Down Expand Up @@ -147,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
Expand All @@ -163,11 +180,17 @@ 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(Φ)
m.Amp[3] = 0.0
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
Expand All @@ -184,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(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),
Expand Down