Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...
Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...
Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...
You also want an ePaper? Increase the reach of your titles
YUMPU automatically turns print PDFs into web optimized ePapers that Google loves.
pty:=roy+dy;<br />
ptz:=roz+dz;<br />
end;<br />
function MyPoint.AbsDifference : double;<br />
begin<br />
AbsDifference := sqrt(sqr(tx-x)+sqr(ty-y)+sqr(tz-z));<br />
end;<br />
function MyPoint.Alpha : double;<br />
begin<br />
Alpha:=180/pi*ArcTan2(ptz,(ptx*py/r1-pty*px/r1));<br />
end;<br />
procedure InitConstParam;<br />
var<br />
code:integer;<br />
a1,a2,a3:double;<br />
begin<br />
a1:=90*pi/180;<br />
a2:=(90+120)*pi/180;<br />
a3:=(90+240)*pi/180;<br />
Val(InvMainForm.EditR1.Text,r1,code);<br />
if code 0 then ShowMessage('Illegal entry for R1');<br />
Val(InvMainForm.EditR2.Text,r2,code);<br />
if code 0 then ShowMessage('Illegal entry for R2');<br />
BasePoint[1].x:=r1*cos(a1);<br />
BasePoint[2].x:=r2*cos(a1);<br />
BasePoint[3].x:=r1*cos(a2);<br />
BasePoint[4].x:=r2*cos(a2);<br />
BasePoint[5].x:=r1*cos(a3);<br />
BasePoint[6].x:=r2*cos(a3);<br />
BasePoint[1].y:=r1*sin(a1);<br />
BasePoint[2].y:=r2*sin(a1);<br />
BasePoint[3].y:=r1*sin(a2);<br />
BasePoint[4].y:=r2*sin(a2);<br />
BasePoint[5].y:=r1*sin(a3);<br />
BasePoint[6].y:=r2*sin(a3);<br />
for code:=1 to 6 do BasePoint[code].z:=0;<br />
end;<br />
Procedure CalculateInputParameters;<br />
begin<br />
InvMainForm.EditP1.Text:=Format('%8.3f',[BasePoint[2].AbsDifference]);<br />
InvMainForm.EditP2.Text:=Format('%8.3f',[BasePoint[4].AbsDifference]);<br />
InvMainForm.EditP3.Text:=Format('%8.3f',[BasePoint[6].AbsDifference]);<br />
InvMainForm.EditM1.Text:=Format('%4.2f',[BasePoint[1].Alpha]);<br />
InvMainForm.EditM2.Text:=Format('%4.2f',[BasePoint[3].Alpha]);<br />
InvMainForm.EditM3.Text:=Format('%4.2f',[BasePoint[5].Alpha]);<br />
end;<br />
Procedure DoInverseTask;<br />
var<br />
i:integer;<br />
scale: real;<br />
begin<br />
InitConstParam;<br />
ReadTransformationParameters;<br />
For i:=1 to 6 do<br />
BasePoint[i].Transform(roll,pitch,yaw,rx,ry,rz);<br />
CalculateInputParameters;<br />
75