Advanced Search
All Posts : Applications, Examples and Libraries

A question was raised recently on Stewart Gough platforms.  I decided to tidy up some old code to show platform position and leg lengths for any given displacement.
 

restart

``

Hexapod Setup Data

 

RotZ := proc (delta) options operator, arrow; Matrix(1 .. 3, 1 .. 3, {(1, 1) = cos(delta), (1, 2) = -sin(delta), (1, 3) = 0, (2, 1) = sin(delta), (2, 2) = cos(delta), (2, 3) = 0, (3, 1) = 0, (3, 2) = 0, (3, 3) = 1}, datatype = anything, storage = rectangular, order = Fortran_order, subtype = Matrix) end proc

a[1] := Vector(3, [.5, 3.0, 0]); a[2] := evalf(RotZ(20*((1/180)*Pi)).a[1]); a[3] := evalf(RotZ(100*((1/180)*Pi)).a[2]); a[4] := evalf(RotZ(20*((1/180)*Pi)).a[3]); a[5] := evalf(RotZ(100*((1/180)*Pi)).a[4]); a[6] := evalf(RotZ(20*((1/180)*Pi)).a[5])

b[1] := evalf(.7*RotZ(-40*((1/180)*Pi)).a[1]); b[2] := evalf(RotZ(100*Pi*(1/180)).b[1]); b[3] := evalf(RotZ(20*Pi*(1/180)).b[2]); b[4] := evalf(RotZ(100*Pi*(1/180)).b[3]); b[5] := evalf(RotZ(20*Pi*(1/180)).b[4]); b[6] := evalf(RotZ(100*Pi*(1/180)).b[5])

Zeroposn := Vector(3, [0, 0, 3])

Tx := Vector(3, [1, 0, 0]); Ty := Vector(3, [0, 1, 0]); Tz := Vector(3, [0, 0, 1])

``

``

NULL

Procedures

 

PlatPosn := proc (x := 0, y := 0, z := 0, alpha := 0, beta := 0, delta := 0) local i, v, Rot, L1, L2, L3, L4, L5, L6; global txn, tyn, tzn, ctrp; description "Calculates the platform position in the Global Coordinates, Unit normals and Leg Lengths"; v := Vector(3, [x, y, z]); ctrp := Zeroposn+v; Rot := Matrix(1 .. 3, 1 .. 3, {(1, 1) = cos(delta)*cos(beta), (1, 2) = -sin(delta)*cos(alpha)+cos(delta)*sin(beta)*sin(alpha), (1, 3) = sin(delta)*sin(alpha)+cos(delta)*sin(beta)*cos(alpha), (2, 1) = sin(delta)*cos(beta), (2, 2) = cos(delta)*cos(alpha)+sin(delta)*sin(beta)*sin(alpha), (2, 3) = -cos(delta)*sin(alpha)+sin(delta)*sin(beta)*cos(alpha), (3, 1) = -sin(beta), (3, 2) = cos(beta)*sin(alpha), (3, 3) = cos(beta)*cos(alpha)}, datatype = anything, storage = rectangular, order = Fortran_order, subtype = Matrix); for i to 6 do bn || i := Zeroposn+v+Rot.b[i] end do; txn := Rot.Tx; tyn := Rot.Ty; tzn := Rot.Tz; print(" Platform centre Global", ctrp); print(" Platform corner Co-ords Global", bn1, bn2, bn3, bn4, bn5, bn6); print("Platform Triad Vectors ", "X green ", txn, "Y blue", tyn, "Z red ", tzn); L1 := sqrt((bn1-a[1])^%T.(bn1-a[1])); L2 := sqrt((bn2-a[2])^%T.(bn2-a[2])); L3 := sqrt((bn3-a[3])^%T.(bn3-a[3])); L4 := sqrt((bn4-a[4])^%T.(bn4-a[4])); L5 := sqrt((bn5-a[5])^%T.(bn5-a[5])); L6 := sqrt((bn6-a[6])^%T.(bn6-a[6])); print("Leg Lengths"); print("L1= ", L1); print("L2= ", L2); print("L3= ", L3); print("L4= ", L4); print("L5= ", L5); print("L6= ", L6) end proc

``

PlatPlot := proc () local Base, Platformdisplacement, picL1, picL2, picL3, picL4, picL5, picL6; global tx0, ty0, tz0; description "Displays the Hexapod"; Base := plots:-polygonplot3d(Matrix([a[1], a[2], a[3], a[4], a[5], a[6]], datatype = float), color = black, transparency = .5); Platformdisplacement := plots:-polygonplot3d(Matrix([seq(bn || i, i = 1 .. 6)]), color = cyan, transparency = .5); picL1 := plots:-arrow(a[1], bn || 1-a[1], colour = green); picL2 := plots:-arrow(a[2], bn || 2-a[2], colour = blue); picL3 := plots:-arrow(a[3], bn || 3-a[3], colour = blue); picL4 := plots:-arrow(a[4], bn || 4-a[4], colour = blue); picL5 := plots:-arrow(a[5], bn || 5-a[5], colour = blue); picL6 := plots:-arrow(a[6], bn || 6-a[6], colour = orange); tx0 := plots:-arrow(ctrp, txn, colour = green); ty0 := plots:-arrow(ctrp, tyn, colour = blue); tz0 := plots:-arrow(ctrp, tzn, colour = red); plots:-display(Base, picL1, picL2, picL3, picL4, picL5, picL6, Platformdisplacement, tx0, ty0, tz0, axes = box, labels = [X, Y, Z], scaling = constrained) end proc

``

NULL

``

``

PlatPosn()

"L6= ", 3.586394355

 (1)

PlatPlot()

  

NULL

PlatPosn(.52, -.89, .7, .2, .67, .3)

"L6= ", 3.055217994

 (2)

PlatPlot()

  

NULL

NULL

 

NULL

print('tzn' = LinearAlgebra:-CrossProduct(txn, tyn), `= `, tzn)

tzn = Vector[column](%id = 18446744074564617750), `= `, Vector[column](%id = 18446744074328082542)

 (3)

``

``NULL

NULL

Rotation Matrices

NULL

``

 

RotZ := Matrix(3, 3, {(1, 1) = cos(delta), (1, 2) = -sin(delta), (1, 3) = 0, (2, 1) = sin(delta), (2, 2) = cos(delta), (2, 3) = 0, (3, 1) = 0, (3, 2) = 0, (3, 3) = 1})

RotY := Matrix(3, 3, {(1, 1) = cos(beta), (1, 2) = 0, (1, 3) = sin(beta), (2, 1) = 0, (2, 2) = 1, (2, 3) = 0, (3, 1) = -sin(beta), (3, 2) = 0, (3, 3) = cos(beta)})

RotX := Matrix(3, 3, {(1, 1) = 1, (1, 2) = 0, (1, 3) = 0, (2, 1) = 0, (2, 2) = cos(alpha), (2, 3) = -sin(alpha), (3, 1) = 0, (3, 2) = sin(alpha), (3, 3) = cos(alpha)})

> 

NULL

ROT := RotZ.RotY.RotX

Matrix(%id = 18446744074564619310)

 (4)

``

``

``


 

Download Reverse_Kinematics_Stewart_Gough_Platform.mw
plot kinematics rotation

Please Wait...