add two link jump lin id

jump_lin_id/two_link.jl

+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)