|
ABB機器人如果需要補償旋轉(zhuǎn)角，通常需要獲取當(dāng)前姿態(tài)數(shù)據(jù)，將四元素轉(zhuǎn)換為歐拉角，然后將其進行加減處理，處理完成后需要將歐拉角轉(zhuǎn)換回四元素賦值到新的點位坐標(biāo)中進行使用。 
以下文章給大家分享一個簡單的功能塊程序，可以設(shè)置轉(zhuǎn)換前的坐標(biāo)點，RZ需要補償?shù)男D(zhuǎn)角度，輸出一個新的姿態(tài)數(shù)據(jù)。小伙伴們可以結(jié)合思路，自己編寫一個可以隨意補償RX，RY，RZ角度的功能塊程序。
PROC TransPose(\PERS robtarget A\PERS num Angle_RZ)!定義轉(zhuǎn)換前的點位A，以及需要補償RZ的角度值 VAR num OldRZ; !聲明轉(zhuǎn)換前的RZ值 VAR num OldRX; !聲明轉(zhuǎn)換前的RX值 VAR num OldRY; !聲明轉(zhuǎn)換前的RY值 VAR num NewRZ; !聲明轉(zhuǎn)換前的RZ值 VAR Pose B; !聲明新姿態(tài)數(shù)據(jù)B OldRX:= EulerZYX(\X, A.rot); !提取轉(zhuǎn)換前A點姿態(tài)數(shù)據(jù)RX值 OldRY:= EulerZYX(\Y, A.rot); !提取轉(zhuǎn)換前A點姿態(tài)數(shù)據(jù)RY值 OldRZ:= EulerZYX(\Z, A.rot); !提取轉(zhuǎn)換前A點姿態(tài)數(shù)據(jù)RZ值 NewRZ:=Angle_RZ+OldRZ; !將轉(zhuǎn)換前的RZ值+希望補償?shù)闹礎(chǔ)ngle_Z B.rot:=OrientZYX(NewRZ,OldRY,OldRX); !將新的歐拉角數(shù)值合成新的姿態(tài)數(shù)據(jù)B TransRZ.rot:=B.rot; !將更新的姿態(tài)數(shù)據(jù)B賦值給變量TransRZ.rotENDPROC
|
