4 bar linkage filament cutter

4bar2.jl

# %%
using BSplineKit, LinearAlgebra, Optimization
using Plots
using RCall
using SplitApplyCombine
using Symbolics
@rlibrary ggplot2

import OptimizationOptimisers, DataFrames


# %%
function listofvars(expr)
  # Convert to sorted list for consistent output
  return sort(Symbolics.get_variables(expr), by=string)
end

# %%
# I have a 4 bar linkage perhaps with a slider instead of two pins.
# It will make a difference if the model
# included friction between the blade and filament, which might be reduced
# by putting the blade on the outside curve of the filament, so that the
# cut opens up as it is made.
#
# A is the line length between the servo axes; The blade is one the C edge.
# The origin is at the intersection of A and D.
#
#                A
#          O--------------
#         / ADA       ABA \
#        /                 \
#   D   /                   \   B
#      /                     \
#     /                       \
# ---x-----------------------------------------
#                C
#
#     ---------
#    /          \
#   /            \
#  /    (fx,fy)   \
#  \ filament     /
#   \ center     /
#    \          /
#     ---------
#
# The goal is to define ADA(time), ABA(time) over time
# for A,B,C,fx,fy such that the blade cuts the whole
# filament.
@variables ADA ABA D A B F fx fy s t
@variables pdcx pdcy pbcx pbcy npbcd lnx lny perpx perpy intx inty dhp tauad tauab qdblade dblade dblade_aba dblade_ada fy0


dds = e -> expand_derivatives(Differential(s)(e))

# x, y coordinates of the intersection D-C
pdcx = D * sin(ADA)
pdcy = D * cos(ADA)

# x, y coordinates of the intersection B-C
pbcx = A + B * sin(ABA) # sin(pi - ABA)
pbcy = B * (-cos(ABA))  # cos(pi - ABA)

# length of the blade segment c (norm of bc-dc)
npbcd = sqrt((pbcx - pdcx)^2 + (pbcy - pdcy)^2)

# a point along the blade c, parameterized by t for tangential
lnx = pdcx + t * (pbcx - pdcx)
lny = pdcy + t * (pbcy - pdcy)

# a point along the line through the filament center, perpendicular to the blade
# the normalization means that s must be in the ±1.75/2 interval to make progress.
# It does not necessarily make progress because the blade could be traveling in
# space already cut.
perpx = fx - s * (pbcy - pdcy) * sign(pbcx - pdcx) / npbcd
perpy = fy + s * (pbcx - pdcx) * sign(pbcx - pdcx) / npbcd

# Get the parameter values foro the intersection
steq = symbolic_linear_solve([perpx - lnx, perpy - lny], [s, t])

substeq = eq -> substitute(eq, steq .=> [s, t])

# Intersection of the two lines. This is the first point on the blade to contact the filament,
# and if the blade angle doesn't change (much?) over the cycle, it's the last point to finish cutting.
intx = substeq(perpx)
inty = substeq(perpy)

# Signed distance between the filament center (fx,fy) and the blade's half-plane.
dhp = dds(perpx) * (fx - intx) + dds(perpy) * (fy - inty)

# Torque calculations
# If the blade normal force is F (should this be a pressure instead?)
# The lever rule gives the force at the two joints as (1-t)F and tF,
# and the cross product of the force vector and moment arm gives the torque.
crossZ(a1, a2, b1, b2) = a1 * b2 - a2 * b1

print("tauad:")
tauad = crossZ(pdcx, pdcy, dds(perpx), dds(perpy)) * (1 - steq[2]) * F
print("tauab:")
tauab = crossZ(pbcx - A, pbcy, dds(perpx), dds(perpy)) * steq[2] * F

print("listofvars(tauad):")
listofvars(tauad)
print("listofvars(tauab):")
listofvars(tauab)

qdblade = steq[2] * abs(steq[2]) * ((pbcx - pdcx)^2 + (pbcy - pdcy)^2)
print("listofvars(qdblade)")
listofvars(qdblade)
dblade = sign(qdblade) * sqrt(abs(qdblade))
print("listofvars(dblade)")
listofvars(dblade)

print("dblade_aba:")
dblade_aba = expand_derivatives(Differential(ABA)(dblade))

print("dblade_ada:")
dblade_ada = expand_derivatives(Differential(ADA)(dblade))

x = y = LinRange(0, 4, 5)
# %%

f(ABA0, ADA0) = substitute(dblade, [ABA => ABA0, ADA => ADA0, A => 1, B => 1, D => 1, fx => 0.2, fy => 0.2])
xyz = invert([substitute([xi, yi, f(xi, yi)], Dict()) for xi in x, yi in y][:])
d = DataFrames.DataFrame(xyz, :auto)

pdf()
ggplot(d, aes(:x1, :x2, z=:x3)) + geom_contour_filled() + theme_minimal() + xlab("angle ABA") + ylab("angle ADA") + ggtitle("signed distance between the blade and filament center")
R"dev.off()"


# next I have to redo barf from 4bar.R
#
# BSplineKit is straightforward:
# Sper = interpolate(xp, yp, BggSplineOrder(4), Periodic(2))
# Sper(1) is the cubic spline interpolant at 1
#
# I want to minimize peak torque (rms?) tauad tauab
# while s->-1.75/2 to 1.75/2
# ADA=th, ABA=sqrt(2)*th, then with a big enough th sampling, all possible rotations are covered
# maybe I should stop substeq instead have more constraints
# instead of solving the path, just ensure that end-points exist?
#
# that is, in the plot I just want to know that there are points
# with the right range of dblade
# ie. solve for angles that lead to min and max dblade,
# but I want to set these to a certain value.
#
# for both joints pinned npbcd = C is constant
# and the min/max dblade plot may include infeasible options where the razor blade has to be longer than 40mm
# the link can be longer though.

# https://docs.sciml.ai/Optimization/stable/tutorials/constraints/
# optf = OptimizationFunction(f, Optimization.AutoZygote())
# prob = OptimizationProblem(optf, x0, _p)
# sol = solve(prob, OptimizationOptimisers.Adam(0.05), maxiters=1000, progress=false)