|
|
> No.3358[元記事へ]
lark12_longさんへのお返事です。
> nアーム回動リンク
擬似逆行列で求めることができるようです。
FULL BASIC仕様のコーディングにしてあります。
!平面nリンクマニピュレー タのヤコビアンを求めよ。
SET WINDOW -16,16,-16,16 !表示領域 ※ワールド座標系
LET N=4 !nリンク
DIM L(N) !各腕の長さ
DATA 8,5,5,3
MAT READ L
DIM Q(N) !各関節の角度 ※単位は度、ワールド座標系
DATA 30,30,30,30
MAT READ Q
CALL PrintOut(N,L,Q, PX,PY,QQ)
DO
mouse poll mx,my,left,right !目標の位置(x,y)
IF right=1 THEN STOP !右ボタンが押下されるまで
!逆運動学
DIM J(2,N) !ヤコビアン
LET J(1,N)=-L(N)*SIN(RAD(QQ))
LET J(2,N)= L(N)*COS(RAD(QQ))
FOR i=N-1 TO 1 STEP -1
LET QQ=QQ-Q(i+1)
LET J(1,i)=J(1,i+1)-L(i)*SIN(RAD(QQ))
LET J(2,i)=J(2,i+1)+L(i)*COS(RAD(QQ))
NEXT i
DIM JT(N,2),W(2,2),JJ(N,2)
MAT JT=TRN(J)
MAT W=J*JT
MAT W=INV(W)
MAT JJ=JT*W !擬似逆行列A^+=A^t*(A*A^t)^(-1)
DIM r(2) !位置誤差
LET r(1)=mx-PX
LET r(2)=my-PY
DIM dQ(N) !⊿θ=kJ^(-1)⊿x
MAT dQ=JJ*r
FOR i=1 TO N !θ←θ+⊿θ
LET Q(i)=MOD(Q(i)+5*dQ(i),360)
NEXT i
CALL PrintOut(N,L,Q, PX,PY,QQ) !手先位置
LOOP
END
EXTERNAL SUB PrintOut(N,L(),Q(), PX,PY,QQ) !順運動学で手先位置を算出する。表示する
SET DRAW mode hidden !ちらつき防止の開始
CLEAR
DRAW grid(2,2) !座標
LET PX=0 !原点(0,0)
LET PY=0
LET QQ=0 !Σθ
FOR i=1 TO N !n個の関節
LET QQ=QQ+Q(i)
LET X=PX+L(i)*COS(RAD(QQ)) !x=L[1]cos(Q[1])+L[2]cos(Q[1]+Q[2])+ …
LET Y=PY+L(i)*SIN(RAD(QQ)) !y=L[1]sin(Q[1])+L[2]sin(Q[1]+Q[2])+ …
PLOT LINES: PX,PY; X,Y
PLOT TEXT ,AT PX,PY: USING$("####.##",Q(i))&"°"
LET PX=X !次へ
LET PY=Y
NEXT i
SET DRAW mode explicit !ちらつき防止の終了
END SUB
|
|