【Processing】平面二軸ロボットの逆運動学

こんにちは。
　前回の記事”【Processing】平面二軸ロボットの順運動学”では各関節の角度を指定して手先の位置を決める順運動学をProcessingを使って実装しました。今回の記事では手先位置を指定して各関節の角度を決定する逆運動学をProcessingを使って実装します。

　逆運動学を解くと以下のようになり、手先位置XE,YE及びリンク長L1,L2より、各関節角度θ1、θ2を決定することができます。実際には同じ手先位置を実現するロボットアームの姿勢が２つ以上存在することがあります。その場合は静力学、動力学を考慮して拘束条件を設定し、各関節の負担を小さくしたりする必要があります。

この式にもとづいて、Processingのプログラムを作成すると以下のようになります。

import controlP5.*;

ControlP5 cp5;
PGraphics pg;
Slider2D ui;

float x1,y1,x2,y2,xe,ye;
float angle1;
float angle2;
float angle1_;
float angle2_;
float the1;
float the2;
final float l1 = 100;
final float l2 = 80;
final int r = 20;
int dir = 1;

void setup(){
  size(640,640);
  stroke(255,255,255);
  fill(255,255,255);
  //set to origin.
  x1 = 300;
  y1 = 350;
  pg = createGraphics(width,height,JAVA2D);
  cp5 = new ControlP5(this);
  
  ui = cp5.addSlider2D("Position")
    .setPosition(30,40)
    .setSize(200,200)
    .setArrayValue(new float[] {15, 30})
    ;
}

void draw()
{ 
  background(0);
  angle1_ = acos((pow(l1,2)-pow(l2,2)+(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2)))/((2*l1)*sqrt(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2))));
  angle1 = atan2(ui.arrayValue()[1],ui.arrayValue()[0])-angle1_;
  angle2_ = acos((pow(l2,2)-pow(l1,2)+(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2)))/((2*l2)*sqrt(pow(ui.arrayValue()[0],2)+pow(ui.arrayValue()[1],2))));
  angle2 = angle1_ + angle2_;
  the1 = angle1*PI/180;
  the2 = angle2*PI/180;
  print(angle1);print(":");println(angle2);
  x2 = x1+l1*cos(angle1);
  y2 = y1+l1*sin(angle1);
  xe = x2+l2*cos(angle1+angle2);
  ye = y2+l2*sin(angle1+angle2);
  ellipse(x1,y1,r,r);
  ellipse(x2,y2,r,r);
  fill(255,0,0);
  ellipse(xe,ye,r,r);
  fill(255);
  line(x1,y1,x2,y2);
  line(x2,y2,xe,ye);
  pg.beginDraw();
    pg.noStroke();
    pg.fill(0,100,100);
    pg.ellipse(xe,ye,r,r);
  pg.endDraw();
  image(pg.get(0,0,width,height),0,0);
}

逆運動学を使用すると、手先のX、Y座標を指定してアーム姿勢を決定するため、以下のように手先をX方向のみに移動させることやY方向のみに移動することができます。

_kiitaniさん(@monitorgazer)が投稿した動画 - 12月 12, 2014 at 12:22午後 PST

また、マウスパッドのようにXY座標を同時に入力するインターフェースを用いると、手先位置をXY軸で自由に動かすことができるので、絵を描いたりすることができます。

_kiitaniさん(@monitorgazer)が投稿した動画 - 12月 12, 2014 at 12:33午後 PST

次回はこのプログラムを基に、Arduinoとサーボモータを使用して、実際に平面二軸ロボットを作ります。