From a278bbfd7751dccbd8092be240c87be745027eea Mon Sep 17 00:00:00 2001
From: baggepinnen <cont-frb@ulund.org>
Date: Tue, 13 Feb 2018 19:17:06 +0100
Subject: [PATCH] add two link jump lin id
---
jump_lin_id/two_link.jl | 97 +++++++++++++++++++++++++++++++++++++++++
1 file changed, 97 insertions(+)
create mode 100644 jump_lin_id/two_link.jl
diff --git a/jump_lin_id/two_link.jl b/jump_lin_id/two_link.jl
new file mode 100644
index 0000000..be57768
--- /dev/null
+++ b/jump_lin_id/two_link.jl
@@ -0,0 +1,97 @@
+cd(@__DIR__)
+# import Iterators
+using OrdinaryDiffEq, LTVModels
+
+using ValueHistories, IterTools, MLDataUtils, OrdinaryDiffEq, Parameters, InteractNext
+
+inoffice() = gethostname() ∈ ["billman", "Battostation"]
+inoffice() && !isdefined(:time_derivative) && include(Pkg.dir("DynamicMovementPrimitives","src","two_link.jl"))
+if !isdefined(:AbstractEnsembleSystem)
+ inoffice() || @everywhere include("/var/tmp/fredrikb/v0.6/DynamicMovementPrimitives/src/two_link.jl")
+ @everywhere include("../flux/nn_prior/utilities.jl")
+ @everywhere include("../flux/nn_prior/two_link_sys.jl")
+ @everywhere include("../flux/nn_prior/linear_sys.jl")
+end
+
+n,m,N,h,σ0 = 4,2,200,0.02,0.01
+np = n*(n+m)
+sys = TwoLinkSys(N=N, h=h, σ0 = σ0)#; warn("Added noise")
+seed = 1
+
+ni = N+2
+traj = connect_points([0 -0.5; 0.9 -0.1],ni)
+q = inverse_kin(traj[1],:up)
+p = forward_kin(q)
+qd = centraldiff(q')'/sys.h
+qdd = centraldiff(qd')'/sys.h
+u = torque(q,qd,qdd)
+u .+= 0.2randn(size(u))
+# x,y,u = generate_data(sys, System, seed; ufun=x->u)
+
+const wall = -0.2
+function affect!(i)
+ q = i.u[1:2]
+ qd = i.u[3:4]
+ p1,p2 = forward_kin(q)[2]
+ J = TwoLink.jacobian(q)
+ v1,v2 = J*qd
+ d = [0, wall-p2]
+ δq = J\d
+ i.u[3:4] = J\[v1, 0]
+ i.u[1:2] .+= δq
+end
+condition(t,x,i) = forward_kin(x)[2][2] > wall
+cb = DiscreteCallback(condition,affect!)
+
+t = h:h:N*h+h
+q0 = [q[:,1]; qd[:,1]]
+prob = OrdinaryDiffEq.ODEProblem((t,x)->time_derivative(x, u[:,ceil(Int,t/h)]),q0,(t[[1,end]]...))
+sol = solve(prob,Tsit5(),reltol=1e-8,abstol=1e-8, callback=cb)
+x = hcat(sol(t)...)
+u = u[:,1:N]
+x,y = xy(System,x,N)
+plot(forward_kin(x[1:2,:])')
+plot!([1,N],[wall, wall], l=:dash, c=:black)
+error("True jacobian påverkas inte av väggen :/ Den syns bara i datan. Kolla om breakpoint identifieras när systemet träffar väggen")
+##
+evalpoints = 1:sys.N
+Jtrue = map(evalpoints) do evalpoint
+ true_jacobian(sys, evalpoint, x, u)
+end
+Jtrue = cat(3, Jtrue...)
+Jtrue = reshape(Jtrue, np, sys.N)
+colors = [HSV(h,1,0.8) for h in linspace(0,200,5)]
+
+r2,p0 = 300,7
+model = LTVModels.fit_statespace_admm(x,u,1, extend=false)
+ui = @manipulate for i in 1:24, λ in linspace(-3,3,50)
+ plot(Jtrue[i,:],lab="True", l=:dash, c=:blue, layout=1, legend=false, size=(1920,1200), show=false)
+ # ui = @manipulate for r2 in linspace(1,1000,20), p0 in linspace(1,7,50), i in 1:24
+ global n,m,np,x,u,N,model
+ function dostuff()
+ j = 1
+ R1 = eye(n*(n+m))
+ R2 = r2*eye(n)
+ P0 = 10^p0*R1
+ model = LTVModels.fit_statespace_admm!(model,x,u,10^λ, extend=false, tol=1e-4)
+ # model = LTVModels.fit_statespace_dp(x,u,M,extend=false)
+ # model = LTVModels.fit_model(LTVModels.KalmanModel, x,u,R1,R2,P0, extend=true)
+ seed = 1
+ J = reshape(cat(2, model.At, model.Bt), np, N)
+
+ # yh = LTVModels.predict(model, x,u)
+ # plot(y', lab="y", layout=6, show=false)
+ # plot!(yh', lab="yh", subplot=1:4, show=false)
+ # plot!(u', lab="u", subplot=5:6)
+ plot!(J[i,:], lab="Estimated", c=colors[j], subplot=1, show=false, show=false)
+ plot!(repmat([1, N],1,N), [0,0], l=:dash, c=:black, linewidth=2)
+ end
+ dostuff()
+end
+# gui()
+
+
+##
+
+w = get_page()
+body!(w, ui)
--
GitLab