fkine, ikine, jacobian = get_kinematic_functions("yumi") # Replace yumi for your robot model, as long as it's supported
data = csv2dict(path) # Read data from a csv-file and store in a dict
q = getdata("robot_0.*posRawAbs", data, 1, removeNaN = false) # Extract columns from data object using regex like syntax

using Robotlib
using DSP # For filtfilt

# Define robot to use, in this case YuMi
dh = DHYuMi()
fkine, ikine, jacobian = get_kinematic_functions("robotname")

q,q̇,τ = load_your_data()

# Apply gear ratio transformation
q = q*dh.GR'
q̇ = q̇*dh.GR'
τ = τ*inv(dh.GR')

# Filter velocities to get accelerations
q̈ = filtfilt(ones(50),[50.],centralDiff(q̇))

# plot(abs([q̇, q̈]))

# Sort out data with low acceleration
lowAcc  = all(abs.(q̈) .< 3e-4,2)
q       = q[lowAcc,:]
q̇       = q̇[lowAcc,:]
τ       = τ[lowAcc,:]
f       = f[lowAcc,:]
N       = size(q,1)

# Apply forward kinematics to get end-effector poses
T  = cat([fkine(q[i,:]) for i = 1:N]..., dims=3)

trajplot(T) # Plots a trajectory of R4x4 transformation matrices

# Perform the force sensor calibration and plot the errors
Rf,m,offset     = calib_force(T,f,0.2205,offset=true) # See also calib_force_iterative, calib_force_eigen
err = hcat([Rf*f[i,1:3] + offset - T[1:3,1:3,i]'*[0, 0, m*-9.82] for i = 1:N]...)'
plot(f[:,1:3],lab="Force")
plot!(err,l=:dash,lab="Error")
println("Error: ", round(rms(err), digits=4))

names(Robotlib)
names(Robotlib.Frames)

dh = DH7600() # ABB Irb 7600
xi = DH2twistsPOE(dh)
T  = fkinePOE(xi, q)

dh = DH7600()
Jn, J0, T, Ti, trans = jacobian(q, dh)

using Frames
import MAT
function setupframes(path)
	path = Pkg.dir("Robotlib","src","applications","frames/")

	# Add frame names to the dictionary
	add_frame_name!("SEAM","Weld seam frame")
	add_frame_name!("TAB","Table frame")

	# Read matrices from file
	T_RB_Tm = MAT.matread(path*"T_RB_T.mat")["T_RB_T"]
	T_TF_TCPm = MAT.matread(path*"T_TF_TCP.mat")["T_TF_TCP"]
	T_T_TABm = MAT.matread(path*"T_T_Table.mat")["T_T_Table"]

	# Create frames from matrices
	T_RB_T = Frame(T_RB_Tm,"RB","T")
	T_S_D = Frame(T_TF_TCPm,"S","D")
	T_T_TAB = Frame(T_T_TABm,"T","TAB")

	# Read point clouds generated by nikon software from file
	cloud_seam = readcloud(path*"CloudSeam_edge.txt")
	plane_seam = readplane(path*"PlaneSeam_edge.txt")

	# Project points onto plane and fit a line
	cloud_seam_projected = project(plane_seam,cloud_seam)
	line_seam = fitline(cloud_seam_projected)

	# Create a frame from the measured features
	T_T_SEAM = framefromfeatures(("z+",line_seam),("y-",plane_seam),cloud_seam_projected[1],"SEAM")
	T_RB_SEAM = T_RB_T*T_T_SEAM
	T_RB_TAB = T_RB_T*T_T_TAB
	T_TAB_SEAM = inv(T_T_TAB)*T_T_SEAM


	cloud_seam_RB = T_RB_T*cloud_seam
	cloud_seam_projected_RB = T_RB_T*cloud_seam_projected
	plane_seam_RB = T_RB_T*plane_seam
	line_seam_RB = T_RB_T*line_seam

	# Plot results
	plot(Frame(I4,"RB","U"), 200)
	plot!(cloud_seam_RB, c=:blue)
	plot!(cloud_seam_projected_RB, c=:red)
	plot!(line_seam_RB, 500, label="Line seam")
	plot!(plane_seam_RB, 200, label="Plane seam")
	plot!(T_RB_SEAM, 200, label="T_RB_SEAM")
	plot!(T_RB_TAB, 200, label="T_RB_TAB")

	xlabel!("x")
	ylabel!("y")
	# zlabel!("z")

    # Write frames to file
    MAT.matwrite(path*"T_TAB_SEAM.mat",["T_TAB_SEAM" => T_TAB_SEAM.T])
    MAT.matwrite(path*"T_T_SEAM.mat",["T_T_SEAM" => T_T_SEAM.T])
    MAT.matwrite(path*"T_RB_TAB.mat",["T_RB_TAB" => T_RB_TAB.T])
    println("Wrote T_TAB_SEAM, T_T_SEAM, T_RB_TAB to files in $path")
end