プログラム製作時の楽屋裏 - ロボット制御のための逆運動学

　今日はジェネラティブアートの話ではなく、Processing は数式の検証にも使える - という話をしたい。2 リンク機構のロボットの場合、余弦定理を用いて計算すると、位置を指定しただけで関節部分にあるサーボモータをどれだけ回転させれば良いのか、といったことが一意に得られる。

ちなみに 2 リンク機構のロボットというのは冒頭に掲載されているようなもので、脚部分が 2 つのリンク（棒 2 つで）構成されれいるものを指す。

　この導出はそれほど難しくはない。字が汚くて恐縮であるが、以下のような感じで簡単に計算できる：

…とまあ、こんな感じにノートを作って、研究資料（と言えるのかどうか分からないが）として書き溜めているわけである。

　で、これで本当に正しいかどうかを検証するために Processing を用いている。実際に作ったプログラムはこのようなものだ。

def setup():
    size(500,500)
    noFill()

def draw():
    background(-1)
    strokeWeight(2)
    x0,y0=50,50
    line(0,y0,500,y0)
    line(x0,0,x0,500)
    a=150.0
    b=200.0
    circle(x0,y0,(a+b)*2)
    d=mouseX-x0
    h=mouseY-y0
    stroke(0)
    if d*d+h*h>(a+b)*(a+b):
        s=sqrt(pow(a+b,2)/(d*d+h*h))
        d=d*s
        h=h*s
        stroke(255,0,0)
    circle(x0+d,y0+h,10)
    phi=acos((h*h+d*d-a*a-b*b)/(-2*a*b))
    
    A=a+b*(sin(phi)-cos(phi))
    # B=a-b*(sin(phi)+cos(phi))
    B=-a+b*(sin(phi)+cos(phi))
    t=1/sqrt(A*A+B*B)
    theta=-asin(B*t)+asin((h-d)*t)
    qx=x0+a*cos(theta)
    qy=y0+a*sin(theta)
    rx=qx-b*cos(phi-theta)
    ry=qy+b*sin(phi-theta)
    strokeWeight(5)
    line(x0,y0,qx,qy)
    line(qx,qy,rx,ry)

動かしてみると、ちゃんと動作していることが分かる。導出した式は正しい。これでロボットに組み込むことができる。

逆運動学（インバースキネマティクス）のデモ、少しアップデートした。これのプログラムリストも近日中に note で公開するか…。 pic.twitter.com/2Opte3V6uk

— Koji Saito (@KojiSaito) March 4, 2022

　滑らかに動かすためには、おそらく C2 級であることなどを考慮しなければならないと思う。しかし、安価なサーボでどこまでできるのか…という気持ちもある。予算に余裕があるなら、V-Sido を購入した方が良い気もする（が、そんな予算はないのであった。トホホ…いや、やれやれ、と書くべきか）。

　以上、プログラム製作時の楽屋裏の紹介であった。