Symbolically derive forward and inverse kinematic equation out both configration and motion level

for te Pantograf.

The end effector is located at the intersection of B and C and marked as point E.

L1=L4 , L2=L3

POW= x|n1 + y|n2 fixed positon vector from O to the center of the workspace.
rα=rx|nx + ry|ny
Holonomic Constraint Equation:
L1.|a1 + L2. |b1 – L3. |c1 – L4. |d1 – L0. |n1=0

Relation Between Frames

a|1 a|2 a|3 b|1 b|2 b|3

ln1 Cos θ1 -Sin θ1 0 ln1 Cos θ2 - Sin θ2 0

ln2 Sin θ1 Cos θ1 0 ln2 Sin θ2 Cos θ2 0

ln3 0 0 1 ln3 0 0 1

c|1 c|2 c|3 d|1 d|2 d|3

ln1 Cos θ3 -Sin θ3 0 ln1 Cos θ4 - Sin θ4 0

ln2 Sin θ3 Cos θ3 0 ln2 Sin θ4 Cos θ4 0

ln3 0 0 1 ln3 0 0 1
Angular velocities of Bodies :

N
wA = θ1|k N
wC = θ3|k
by inspection since all rotations are simple rotations
N
wB = θ2|k N
wD = θ4|k

Configration Level Forward Kinematics

Given θ1 and θ4 solve for x,y of point E.

|pOE = x|n1 + y|n2
= L1.|a1 + L2. |b1
= L1( Cos θ1|n1 + Sin θ1|n2 ) + L2( Cos θ2|n1 + Sin θ2|n2 )
= ( L1Cos θ1 + L2Cos θ2 ) |n1 + ( L1Sin θ1 + L2Sin θ2 ) |n2

xs= L1Cos θ1 + L2Cos θ2

y= L1Sin θ1 + L2Sin θ2

Now we need to solve for θ2 in terms of θ1 using the holonomic constraint equation.

L1( Cos θ1|n1 + Sin θ1|n2 ) + L2( Cos θ2|n1 + Sin θ2|n2 )

- L3( Cos θ3|n1 + Sin θ3|n2 ) + L4( Cos θ4|n1 + Sin θ4|n2 ) – L0|n1=0

L1Cos θ1 + L2Cos θ2 – L3Cos θ3 – L4Cos θ4 = 0 (1)

L1Sin θ1 + L2Sin θ2 – L3Sin θ3 – L4Sin θ4 = 0 (2)

Solve Equation (1) and Equation (2) for θ2 and θ3 using Newton-Raphson method.
𝑓1
F=
𝑓2
Where f1= L1Cos θ1 + L2Cos θ2 – L3Cos θ3 – L4Cos θ4 – L0

f2= L1Sin θ1 + L2Sin θ2 – L3Sin θ3 – L4Sin θ4

𝜃1
Since θ1 and θ4 and all link lengths are known. x =
θ2

𝑑𝐹 𝑑𝐹
𝑑𝐹 𝑑θ1 𝑑θ1
f(x,y) = 𝑑𝑥 = = −𝐿2𝑠𝑖𝑛𝜃2 𝐿3𝑠𝑖𝑛𝜃3
𝑑𝐹 𝑑𝐹 𝐿2𝑐𝑜𝑠𝜃2 −𝐿3𝑠𝑖𝑛𝜃3
𝑑θ1 𝑑θ1

f(x)= f(x0)+ f’(x)(x-x0)+ f’’(x) (x-x0)2+ f’’’(x)(x-x0)3 + ...

~0
Suppose that the initial guess is pretty close to the real root. Then (x-x0) is small and only the first few
terms in the series are important to pet an accurate estimate.

f(x)= f(x0)+ f’(x)(x-x0)+ f’’(x) (x-x0)2

For f(x) = 0 solve for x = x0 – f(x0) / f’(x)

Newton Raphson gösterimi

xi+1 = xi – f(x0) / f’(x)

𝑥1 𝑥1 𝑓1(𝑥0, 𝑦0)
= = [ j(x,y) ]-1
𝑦1 𝑦1 𝑓2(𝑥0, 𝑦0)

Configration Level Inverse Kinematics

Given x,y of point E, find θ1 and θ4

|pOE = x|n1 + y|n2 x - L1Cos θ1 - L2Cos θ2 = 0 (1)

= L1.|a1 + L2. |b1 y - L1Sin θ1 - L2Sin θ2 = 0 (2)

and from the holonomic constraint

L1Cos θ1 + L2Cos θ2 – L3Cos θ3 – L4Cos θ4– L0 = 0 (3)

L1Sin θ1 + L2Sin θ2 – L3Sin θ3 – L4Sin θ4 = 0 (4)

Solve Equation (1) and (4) for θ1, θ2, θ3, θ4 using Newton Raphson
(1) (θ1)
(2) (θ2)
F = [ ] , |x = [ ]
(3) (θ3)
(4) (θ4)

L1Sin θ1 L2Sin θ2 0 0
𝑑𝐹 −L1Cos θ1 −L2Cos θ2 0 0
=[ ]
𝑑𝑥 −L1Sin θ1 −L2Sin θ2 L3Sin θ3 L4Sin θ4
L1Cos θ1 L2Cos θ2 −L3Cos θ3 −L4Cos θ4

Motion Level Forward Kinematics

𝑁𝑑/𝑑𝑡 |𝑃𝑂𝐸 = 𝑁𝑉|𝐸 = 𝑥|𝑛1 + 𝑦|𝑛2
𝑁𝑑/𝑑𝑡 |𝑃𝑂𝐸 = 𝑁𝑑/𝑑𝑡 ( 𝐿1. |𝑎1 + 𝐿2. |𝑏1 ) = 𝐿1 𝑁𝑑/𝑑𝑡|𝑎1 + 𝐿2 𝑁𝑑/𝑑𝑡|𝑏1
= 𝐿1 𝜃̇ 1|𝑘 . |𝑎1 + 𝐿2 𝜃̇ 2|𝑘 . |𝑏1
= 𝐿1 𝜃̇ 1|𝑎2 + 𝐿2 𝜃̇ 2 . |𝑏2
= 𝐿1 𝜃 ̇1( −𝑆𝑖𝑛 𝜃1 |𝑛1 + 𝐶𝑜𝑠 𝜃1 |𝑛2 ) + 𝐿2 𝜃 ̇2( −𝑆𝑖𝑛 𝜃2 |𝑛1 + 𝐶𝑜𝑠 𝜃2 |𝑛2 )
𝑥 ̇ = −𝐿1𝑆𝑖𝑛 𝜃1 𝜃 ̇1 − 𝐿2 𝑆𝑖𝑛 𝜃2 𝜃 ̇2 , 𝑦 = 𝐿1𝑆𝑖𝑛 𝜃1 𝜃1 + 𝐿2 𝑆𝑖𝑛 𝜃2 𝜃 ̇2
Once 𝜃 ̇2 is solved in terms of 𝜃 ̇1 and 𝜃 ̇4, motion level formula kinematics is complete.
𝑁𝑑/𝑑𝑡(( 𝐿1. |𝑎1 + 𝐿2. |𝑏1 − 𝐿3. |𝑐1 − 𝐿4. |𝑑1 − 𝐿0. |𝑛1 = Ø1
N
wA x 𝐿1. |𝑎1 + NwB x 𝐿2. |𝑏1 - NwC x 𝐿3. |𝑐1 - NwD x 𝐿4. |𝑑1 = Ø

𝐿1 𝜃̇ 1|𝑎2 + 𝐿2 𝜃̇ 2|𝑏2 − 𝐿3 𝜃 ̇3|𝑐2 − 𝐿4 𝜃 ̇4|𝑑2 = Ø

𝐿1 𝜃 ̇1( −𝑆𝑖𝑛 𝜃1 |𝑛1 + 𝐶𝑜𝑠 𝜃1 |𝑛2 ) + 𝐿2 𝜃 ̇2( −𝑆𝑖𝑛 𝜃2 |𝑛1 + 𝐶𝑜𝑠 𝜃2 |𝑛2 )
−𝐿3 𝜃 ̇3( −𝑆𝑖𝑛 𝜃3 |𝑛1 + 𝐶𝑜𝑠 𝜃3 |𝑛2 ) + 𝐿4 𝜃 ̇4( −𝑆𝑖𝑛 𝜃4 |𝑛1 + 𝐶𝑜𝑠 𝜃4 |𝑛2 ) = 0

−𝐿1𝑆𝑖𝑛 𝜃1 𝜃̇ 1 − 𝐿2 𝑆𝑖𝑛 𝜃2 𝜃̇ 2 + 𝐿3𝑆𝑖𝑛 𝜃3 𝜃̇ 3 + 𝐿4 𝑆𝑖𝑛 𝜃4 𝜃̇4 = 0

𝐿1𝐶𝑜𝑠 𝜃1 𝜃̇ 1 + 𝐿2 𝐶𝑜𝑠 𝜃2 𝜃̇ 2 − 𝐿3𝐶𝑜𝑠 𝜃3 𝜃̇ 3 − 𝐿4 𝐶𝑜𝑠 𝜃4 𝜃̇ 4 = 0

𝐿2 𝑆𝑖𝑛 𝜃2 𝐿3𝑆𝑖𝑛 𝜃3 𝜃̇ 2 𝐿1𝑆𝑖𝑛 𝜃1 −𝐿4 𝑆𝑖𝑛 𝜃4

[ ][ ] = [ ]
𝐿2 𝐶𝑜𝑠 𝜃2 ̇
−𝐿3𝐶𝑜𝑠 𝜃3 𝜃 3 𝐿1𝐶𝑜𝑠 𝜃1 𝐿4 𝐶𝑜𝑠 𝜃4