Bennett linkage animation

(Notation according to MMT paper)

Off[General::spell]
Off[General::spell1]

<< Graphics`Animation`

Use Standard Frame to Compute CB and PQ

Displacement Information

Xdisp[alpha_,  
        a_] := {{1, 0, 0, a}, {0, Cos[alpha], -Sin[alpha], 0}, {0, 
          Sin[alpha], Cos[alpha], 0}, {0, 0, 0, 1}}

Ydisp[beta_,  
        b_] := {{Cos[beta], 0, Sin[beta], 0}, {0, 1, 0, b}, {-Sin[beta], 
          0, Cos[beta], 0}, {0, 0, 0, 1}}

Zdisp[theta_,  
        d_] := {{Cos[theta], -Sin[theta], 0, 0}, {Sin[theta], Cos[theta], 
          0, 0}, {0, 0, 1, d}, {0, 0, 0, 1}}

Define the Tetrahedron

a =. ; g =. ; γ =. ; α =. ; θ0 =. ; ϕ0 =. ; 
    t =. ;

Vec[a_] := {a[[1]],  a[[2]], a[[3]]}

pointB = {0, 0, 0, 1}

pointQ = {g,  0, 0, 1}

pointP = Zdisp[θ0, 0] . {a, 0, 0, 1}

this is equivalent to

pointP = Zdisp[θ0, 0] . Xdisp[0, a] . {0, 0, 0, 1}

Use the fact that ψ=-ϕ, we compute C going through both RR chains:

pointCa = 
      Xdisp[γ, g] . Zdisp[ϕ0, 0] . {a, 0, 0, 1}

pointCb = 
      Zdisp[θ0, 0] . Xdisp[α, a] . 
        Zdisp[ϕ0, 0] . {g,  0,  0,  1}

MatrixForm[pointCa]

MatrixForm[pointCb]

The vector from Q to P:

Lpq = Simplify[Vec[pointQ] - Vec[pointP]]

MatrixForm[Lpq]

The vector from C to B:

Lcb = Simplify[Vec[pointB - pointCa]]

MatrixForm[Lcb]

LpqMag2 = Simplify[Dot[Lpq, Lpq]]

LcbMag2 = Simplify[Dot[Lcb, Lcb]]

tfactor = Simplify[1 - Sqrt[LpqMag2/LcbMag2]]

MidPQ = Simplify[(pointP + pointQ)/2]

MidCB = Simplify[(pointCa + pointB)/2]

Ldist2 = Simplify[Dot[MidPQ - MidCB,  MidPQ - MidCB]]

Analyze Bennett Linkage and compute t, z, and L

Set Bennett Linkage Dimensions

a = 3;  g = 3;  θ = N[30*π/180]

rg = N[180/π]*ArcSin[a/g]

α = N[40*π/180]; sinalpha = Sin[α]

test = (g/a)*sinalpha

γ = π - ArcSin[(g/a)*sinalpha]

N[γ*180/π]

Chop[g*Sin[α] - a*Sin[γ]]

Compute the Input and Output angles

The input-output constraint,

ioconstant = 
      Sin[(α + γ)/2]/
        Sin[(α - γ)/2]

ioc2 = (Tan[α/2] + Tan[γ/2])/(Tan[α/2] - 
            Tan[γ/2])

inc = 10

thetaIn = 
      Table[N[(inc*π/180)*(i - 0.99)],  {i, 1, 32}]

thetaIn = 
      Join[Table[N[(inc*π/180)*(i + 1)],  {i, 1, 15}], 
        Reverse[Table[
            N[(inc*π/180)*(i + 1)],  {i, 1, 15}]]]

nlen = Length[thetaIn]

the coupler angle:

Phi[t_, ik_] = 2*ArcTan[ik*Tan[t/2]]

phiOut = 
      Table[Phi[thetaIn[[i]], ioconstant], {i, 1, nlen}]

More Calculations

mlength = 
      Table[Sqrt[a^2 + g^2 - 2*a*g*Cos[thetaIn[[i]]]], {i, 1, 
          nlen}]

nlength = 
      Table[Sqrt[a^2 + g^2 + 2*a*g*Cos[phiOut[[i]]]], {i, 1, 
          nlen}]

tlist = 
      Table[1 - (mlength[[i]]/nlength[[i]]), {i, 1, 
          nlen}]

L2list = 
      Ldist2 /. {θ0 -> thetaIn, ϕ0 -> 
            phiOut}

Llist = 
      Table[If[L2list[[i]] == 0, 0, 
          L2list[[i]]/Sqrt[Abs[L2list[[i]]]]], {i, 1, 
          nlen}]

Hdist = Table[Llist[[i]]/tlist[[i]], {i, 1, nlen}]

Check α and γ given bennet and io constants

bct = a/g

Eqn = 
      Simplify[((2  x)/(1 + x^2) - 
              bct*(2  y)/(1 + y^2))*(1 + x^2)*(1 + 
              y^2)]

gcnt = (ioconstant - 1)/(ioconstant + 1)

Eqn2 = Simplify[Eqn /. y -> gcnt*x]

Eqn3 = Simplify[Collect[Eqn2/x, x]]

Ans = Solve[Eqn3 == 0, x]

tanA = Table[N[x /. Ans[[i]]], {i, 1, 2}]

Alph = Table[2  ArcTan[tanA[[i]]], {i, 1, 2}]

tanG = gcnt*tanA

Gamm = Table[2  ArcTan[tanG[[i]]], {i, 1, 2}]

Sin[Alph[[2]]]/Sin[Gamm[[2]]]

Calculate r and s

see eq. 45 of MMT.

Acoef = 
      Table[{{tlist[[i]]^2,  (tlist[[i]] - 
                  2)^2}, {(tlist[[i]] - 2)^2, 
            tlist[[i]]^2}}, {i, 1, nlen}]

ndet = Table[Det[Acoef[[i]]], {i, 1, nlen}]

r = 
      Table[Chop[
          Sqrt[Det[{{a^2 - 
                      Hdist[[i]]^2*
                        tlist[[i]]^2,  (tlist[[i]] - 
                          2)^2}, {g^2 - Hdist[[i]]^2*tlist[[i]]^2, 
                    tlist[[i]]^2}}]/ndet[[i]]]], {i, 1, 
          nlen}]

s = 
      Table[Sqrt[
          Chop[Det[{{tlist[[i]]^2, 
                    a^2 - Hdist[[i]]^2*
                        tlist[[i]]^2}, {(tlist[[i]] - 2)^2, 
                    g^2 - Hdist[[i]]^2*tlist[[i]]^2}}]/
              ndet[[i]]]], {i, 1, nlen}]

Compute Vertex Coordinates (the variable t ranges from 0 to 1).

A1 = Table[{r[[i]],  s[[i]],  0}, {i, 1, nlen}]

A2 = Table[{-r[[i]], s[[i]], 0}, {i, 1, nlen}]

A3 = 
      Table[{-r[[i]], -s[[i]], 0}, {i, 1, nlen}]

A4 = Table[{r[[i]], -s[[i]], 0}, {i, 1, nlen}]

B1 = 
      Table[(1 - tlist[[i]])*A1[[i]]  + 
          Hdist[[i]]*{0, 0, tlist[[i]]}, {i, 1, nlen}]

B2 = 
      Table[(1 - tlist[[i]])*A2[[i]]  +  
          Hdist[[i]]*{0, 0, tlist[[i]]}, {i, 1, nlen}]

B3 = 
      Table[(1 - tlist[[i]])*A3[[i]]  + 
          Hdist[[i]]* {0, 0, tlist[[i]]}, {i, 1, nlen}]

B4 = 
      Table[(1 - tlist[[i]])*A4[[i]]  +  
          Hdist[[i]]*{0, 0, tlist[[i]]}, {i, 1, nlen}]

test to get distances a and g:

Atest = 
      Table[Sqrt[(B4[[i]] - A1[[i]]) . (B4[[i]] - 
                A1[[i]])], {i, 1, nlen}]

Gtest = 
      Table[Sqrt[(B2[[i]] - A1[[i]]) . (B2[[i]] - 
                A1[[i]])], {i, 1, nlen}]

Animate the Tetrahedron and Linkage

Without axes

Tetra = 
      Table[Graphics3D[{Thickness[ .01], 
            Line[{A1[[i]], B2[[i]], A3[[i]], B4[[i]], 
                A1[[i]]}], Thickness[ .007], RGBColor[1, 0, 0], 
            Line[{A1[[i]], A2[[i]],  A3[[i]],  A4[[i]], 
                A1[[i]]}], RGBColor[0, 1, 0], 
            Line[{B1[[i]], B2[[i]], B3[[i]], B4[[i]], 
                B1[[i]]}], RGBColor[0, 0, 1], 
            Line[{A1[[i]], B1[[i]]}], 
            Line[{A2[[i]], B2[[i]]}], 
            Line[{A3[[i]], B3[[i]]}], 
            Line[{A4[[i]], B4[[i]]}],  RGBColor[0, 0, 0], 
            PointSize[ .03],  Point[A1[[i]]], Point[B4[[i]]],  
            Point[A3[[i]]],  Point[B2[[i]]]}, 
          PlotRange -> {{-3.5, 3.5}, {-3.5, 3.5}, {-1, 3}}, 
          AspectRatio -> Automatic, 
          ViewPoint -> { .5, -2, 1}], {i, 1, nlen}]

Show[Tetra[[1]]]

Show[Tetra[[2]]]

Animate[Tetra[[k]], {k, 1, nlen, 1}, 
      ImageSize -> 600]

With axes,

Gaxis = 
      Table[((B2[[i]] - A1[[i]])\[Cross](B4[[i]] - 
                  A1[[i]]))/
          Sqrt[((B2[[i]] - A1[[i]])\[Cross](B4[[i]] - 
                      A1[[i]])) . ((B2[[i]] - 
                      A1[[i]])\[Cross](B4[[i]] - 
                      A1[[i]]))], {i, 1, nlen}]

Waxis = 
      Table[((A1[[i]] - B4[[i]])\[Cross](A3[[i]] - 
                  B4[[i]]))/
          Sqrt[((A1[[i]] - B4[[i]])\[Cross](A3[[i]] - 
                      B4[[i]])) . ((A1[[i]] - 
                      B4[[i]])\[Cross](A3[[i]] - 
                      B4[[i]]))], {i, 1, nlen}]

Haxis = 
      Table[((A1[[i]] - B2[[i]])\[Cross](A3[[i]] - 
                  B2[[i]]))/
          Sqrt[((A1[[i]] - B2[[i]])\[Cross](A3[[i]] - 
                      B2[[i]])) . ((A1[[i]] - 
                      B2[[i]])\[Cross](A3[[i]] - 
                      B2[[i]]))], {i, 1, nlen}]

Uaxis = 
      Table[((B4[[i]] - A3[[i]])\[Cross](B2[[i]] - 
                  A3[[i]]))/
          Sqrt[((B4[[i]] - A3[[i]])\[Cross](B2[[i]] - 
                      A3[[i]])) . ((B4[[i]] - 
                      A3[[i]])\[Cross](B2[[i]] - 
                      A3[[i]]))], {i, 1, nlen}]

TetraA = 
      Table[Graphics3D[{Thickness[ .01], 
            Line[{A1[[i]], B2[[i]], A3[[i]], B4[[i]], 
                A1[[i]]}], Thickness[ .005], RGBColor[1, 0, 0], 
            Line[{A1[[i]], A2[[i]],  A3[[i]],  A4[[i]], 
                A1[[i]]}], RGBColor[0, 1, 0], 
            Line[{B1[[i]], B2[[i]], B3[[i]], B4[[i]], 
                B1[[i]]}], RGBColor[0, 0, 1], 
            Line[{A1[[i]], B1[[i]]}], 
            Line[{A2[[i]], B2[[i]]}], 
            Line[{A3[[i]], B3[[i]]}], 
            Line[{A4[[i]], B4[[i]]}],  RGBColor[0, 0, 0], 
            PointSize[ .03],  Point[A1[[i]]], Point[B4[[i]]],  
            Point[A3[[i]]],  Point[B2[[i]]], Thickness[ .012], 
            Line[{A1[[i]] + 0.8  Gaxis[[i]], 
                A1[[i]] - 0.8  Gaxis[[i]]}], 
            Line[{B4[[i]] + 0.8  Waxis[[i]], 
                B4[[i]] - 0.8  Waxis[[i]]}], 
            Line[{B2[[i]] + 0.8  Haxis[[i]], 
                B2[[i]] - 0.8  Haxis[[i]]}], 
            Line[{A3[[i]] + 0.8  Uaxis[[i]], 
                A3[[i]] - 0.8  Uaxis[[i]]}]}, 
          PlotRange -> {{-3.5, 3.5}, {-3.5, 3.5}, {-1, 3}}, 
          AspectRatio -> Automatic, 
          ViewPoint -> { .5, -2, 1}], {i, 1, nlen}]

Animate[TetraA[[k]], {k, 1, nlen, 1}, 
      ImageSize -> 600]

Display two Bennett linkages linked by one R joint

PTrans[M_, v_] := 
      M[[{1, 2, 3}, {1, 2, 3}]] . v + M[[{1, 2, 3}, 4]]

Transform the points A, B, and the axis G, W, H, U

trans2[r_, s_] := 
      Zdisp[Pi/2, 0] . Ydisp[Pi, 0] . Xdisp[0, 2  r] . 
        Ydisp[0, 2  s]

A1t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], A1[[i]]], {i, 1, 
          nlen}]

A2t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], A2[[i]]], {i, 1, 
          nlen}]

A3t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], A3[[i]]], {i, 1, 
          nlen}]

A4t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], A4[[i]]], {i, 1, 
          nlen}]

B1t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], B1[[i]]], {i, 1, 
          nlen}]

B2t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], B2[[i]]], {i, 1, 
          nlen}]

B3t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], B3[[i]]], {i, 1, 
          nlen}]

B4t = 
      Table[PTrans[trans2[r[[i]], s[[i]]], B4[[i]]], {i, 1, 
          nlen}]

Gaxist = 
      Table[((B2t[[i]] - A1t[[i]])\[Cross](B4t[[i]] - 
                  A1t[[i]]))/
          Sqrt[((B2t[[i]] - A1t[[i]])\[Cross](B4t[[i]] - 
                      A1t[[i]])) . ((B2t[[i]] - 
                      A1t[[i]])\[Cross](B4t[[i]] - 
                      A1t[[i]]))], {i, 1, nlen}]

Waxist = 
      Table[((A1t[[i]] - B4t[[i]])\[Cross](A3t[[i]] - 
                  B4t[[i]]))/
          Sqrt[((A1t[[i]] - B4t[[i]])\[Cross](A3t[[i]] - 
                      B4t[[i]])) . ((A1t[[i]] - 
                      B4t[[i]])\[Cross](A3t[[i]] - 
                      B4t[[i]]))], {i, 1, nlen}]

Haxist = 
      Table[((A1t[[i]] - B2t[[i]])\[Cross](A3t[[i]] - 
                  B2t[[i]]))/
          Sqrt[((A1t[[i]] - B2t[[i]])\[Cross](A3t[[i]] - 
                      B2t[[i]])) . ((A1t[[i]] - 
                      B2t[[i]])\[Cross](A3t[[i]] - 
                      B2t[[i]]))], {i, 1, nlen}]

Uaxist = 
      Table[((B4t[[i]] - A3t[[i]])\[Cross](B2t[[i]] - 
                  A3t[[i]]))/
          Sqrt[((B4t[[i]] - A3t[[i]])\[Cross](B2t[[i]] - 
                      A3t[[i]])) . ((B4t[[i]] - 
                      A3t[[i]])\[Cross](B2t[[i]] - 
                      A3t[[i]]))], {i, 1, nlen}]

TetraB = 
      Table[Graphics3D[{Thickness[ .01], 
            Line[{A1[[i]], B2[[i]], A3[[i]], B4[[i]], 
                A1[[i]]}], 
            Line[{A1t[[i]], B2t[[i]], A3t[[i]], B4t[[i]], 
                A1t[[i]]}], Thickness[ .005], RGBColor[1, 0, 0], 
            Line[{A1[[i]], A2[[i]],  A3[[i]],  A4[[i]], 
                A1[[i]]}], 
            Line[{A1t[[i]], A2t[[i]],  A3t[[i]],  A4t[[i]], 
                A1t[[i]]}], RGBColor[0, 1, 0], 
            Line[{B1[[i]], B2[[i]], B3[[i]], B4[[i]], 
                B1[[i]]}], RGBColor[0, 0, 1], 
            Line[{B1t[[i]], B2t[[i]], B3t[[i]], B4t[[i]], 
                B1t[[i]]}], Line[{A1[[i]], B1[[i]]}], 
            Line[{A2[[i]], B2[[i]]}], 
            Line[{A3[[i]], B3[[i]]}], 
            Line[{A4[[i]], B4[[i]]}],  RGBColor[0, 0, 0], 
            PointSize[ .03],  Point[A1[[i]]], Point[B4[[i]]],  
            Point[A3[[i]]],  Point[B2[[i]]], Thickness[ .007], 
            Line[{A1[[i]] + 0.8  Gaxis[[i]], 
                A1[[i]] - 0.8  Gaxis[[i]]}], 
            Line[{B4[[i]] + 0.8  Waxis[[i]], 
                B4[[i]] - 0.8  Waxis[[i]]}], 
            Line[{B2[[i]] + 0.8  Haxis[[i]], 
                B2[[i]] - 0.8  Haxis[[i]]}], 
            Line[{A3[[i]] + 0.8  Uaxis[[i]], 
                A3[[i]] - 0.8  Uaxis[[i]]}], 
            Line[{A1t[[i]] + 0.8  Gaxist[[i]], 
                A1t[[i]] - 0.8  Gaxist[[i]]}], 
            Line[{B4t[[i]] + 0.8  Waxist[[i]], 
                B4t[[i]] - 0.8  Waxist[[i]]}], 
            Line[{B2t[[i]] + 0.8  Haxist[[i]], 
                B2t[[i]] - 0.8  Haxist[[i]]}], 
            Line[{A3t[[i]] + 0.8  Uaxist[[i]], 
                A3t[[i]] - 0.8  Uaxist[[i]]}]}, 
          PlotRange -> {{-7, 7}, {-7, 7}, {-3, 3}}, 
          AspectRatio -> Automatic, 
          ViewPoint -> { .5, -2, 1}], {i, 1, nlen}]

Animate[TetraB[[k]], {k, 1, nlen, 1}, 
      ImageSize -> 600]