|
|
こんなん、作ってみました
ヤコビアン行列の設定について、納得しないままですが、
一応それらしい動きはしてます
文法オプションは、マイクロソフトbasicです
'
'
' nｱｰﾑ回動ﾘﾝｸ.bas
'
' nｱｰﾑﾘﾝｸに於いて、手先目標位置をﾏｳｽで与えた時の、
' ｱｰﾑの動きを、ﾔｺﾋﾞｱﾝ、逆ﾔｺﾋﾞｱﾝ行列を使ってｼﾐｭﾚｰｼｮﾝ
'
'
' H26-04-03 完成
'
'
randomize
width=1260
height=640
set bitmap size width,-height
reft=0
right=width*1.5
bottom=0
top=height*1.5
set window left,right,bottom,top
xbase=700 '表示x基点
ybase=400 '表示y基点
pai=3.14156 '円周率
n=7 '関節、ｱｰﾑ数
option base 1
dim l(n) '各ｱｰﾑ長さ
dim rd(3) '手先目標位置座標 rd(1) x rd(2) y
dim px(n) '各関節のx座標
dim py(n) '各関節のy座標
dim dp(n) '手先目標位置と、手先位置との偏差 dp(1)=rd(1)-px(n) dp(2)=rd(2)-py(n)
dim q(n) 'q(1)=c1 q(2)=c2 q(3)=c3 q(4)=c4... q(n)=cn
dim dq(n) '各角度調整量
dim s(n) 's(i)=sin(q(1)+q(2)+....+q(i))
dim c(n) 'c(i)=cos(q(1)+q(2)+....+q(i))
dim ja(n,n) 'ﾔｺﾋﾞｱﾝ行列
dim invj(n,n) '逆ﾔｺﾋﾞｱﾝ行列
'初期設定
l0=110 '土台固定ｱｰﾑ長さ
for i=1 to n
l(i)=120*(rnd+0.5) 'ｱｰﾑ1～n長さ
next i
'各関節初期角度
for i=1 to n
q(i)=10*pai/180 '度からﾗｼﾞｱﾝに変換
next i
call sici 's(i),c(i)の計算
'逆行列化ｺﾏﾝﾄﾞ、mat invj=inv(ja)が
'使える様にする為にﾔｺﾋﾞｱﾝ行列を,正方行列とする為に、
'ja(3,1)～ja(3,n)、ja(n,1)～ja(n,n)にrndを設定
for i=3 to n
for j=1 to n
ja(i,j)=rnd
next j
next i
call position '関節1～5、手先の位置の計算
call draw '表示
call cal1 'ｼﾐｭﾚｰｼｮﾝ計算
'---------------------------------------------
sub cal1
do while zz=0 '無限ﾙｰﾌﾟ
mouse poll xm,ym,left,right
rd(1)=xm-xbase '手先目標位置x座標
rd(2)=ym-ybase-l0 '手先目標位置y座標
'位置の計算
call position '関節1～n、手先の位置の計算
'位置偏差の計算
dp(1)=rd(1)-px(n) '手先目標位置とのx成分偏差
dp(2)=rd(2)-py(n) '手先目標位置とのy成分偏差
'誤差の評価
pe=dp(1)^2+dp(2)^2
if pe<2 then
call check '誤差範囲に収束したら表示
end if
call sici 's(i),c(i)の計算
'ﾔｺﾋﾞｱﾝ行列を設定
ja(1,n)=-l(n)*s(n)
ja(2,n)=l(n)*c(n)
for i=n-1 to 1 step -1
ja(1,i)=ja(1,i+1)-l(i)*s(i)
ja(2,n)=ja(2,i+1)+l(i)*c(i)
next i
mat invj=inv(ja) 'ﾔｺﾋﾞｱﾝ逆行列計算
kp=0.03 '修正ｹﾞｲﾝ
'各関節角度修正量計算
'dq=kp*invj*dp '行列dq=ｽｶﾗkp*行列invj*行列dp
mat invj=kp*invj
mat dq=invj*dp '各関節角度修正量
'各関節角度の修正更新
mat q=q+dq
for i=1 to n
q(i)=mod(q(i),(pai*2)) '360度以内に正規化
next i
loop
end sub
'---------------------------------------------
sub sici
's(i),c(i)の計算
'関節iの角度を、l(i-1)と成す角度と定義することにより、
'関節iのx軸と成す角度はΣq(i)と表現出来、
's(i)=sin(q(1)+q(2)+....+q(i))
'c(i)=cos(q(1)+q(2)+....+q(i))
'となる
for i=1 to n
cw=0
for j=1 to i
cw=cw+q(j)
next j
s(i)=sin(cw) 's(i)=sin(q(1)+q(2)+...+q(i))
c(i)=cos(cw) 'c(i)=cos(q(1)+q(2)+...+q(i))
next i
end sub
'---------------------------------------------
sub position
'関節1～nのx､y座標を計算、i=nのときは手先のxy座標となる
px(1)=l(1)*c(1)
py(1)=l(1)*s(1)
for i=2 to n
px(i)=px(i-1)+l(i)*c(i)
py(i)=py(i-1)+l(i)*s(i)
next i
end sub
'---------------------------------------------
sub check
'収束したら表示する
call draw
end sub
'---------------------------------------------
sub draw
set draw mode hidden
set line width 4
line(0,0)-(2000,960),1,bf '画面ｸﾘﾔ
line(100,ybase)-(1700,ybase),5 'ﾍﾞｰｽﾗｲﾝ
circle(0+xbase,l0+ybase),10,4,,,,f '固定ｱｰﾑの関節
'各関節表示
for i=1 to n
circle(px(i)+xbase,py(i)+ybase+l0),10,4,,,,f
next i
'各ｱｰﾑの表示
line(xbase,ybase)-(xbase,ybase+l0),5 '固定ｱｰﾑ
'ｱｰﾑ1～n表示
for i=1 to n
line-(px(i)+xbase,py(i)+ybase+l0),6
next i
circle(xm,ym),10,5,,,,f '手先目標位置
'各関節角度表示
xb=1400
yb=900
set text color 5
set text font "MS明朝",20
for i=1 to n
plot text, at xb,yb-50*(i-1):"c"
plot text, at xb+15,yb-50*(i-1),using "##":i
plot text, at xb+50,yb-50*(i-1):"="
cw=0
for j=1 to i
cw=cw+q(j)
next j
cw=mod(cw,(2*pai))*180/pai '360度以内に正規化し、度に変換
plot text, at xb+110,yb-50*(i-1),using"#####.#":cw
next i
set draw mode explicit
end sub
'---------------------------------------------
|
|