Chapter 61

キネマティクス

miku
miku
2021.11.17に更新

この章では人間やロボットなどの関節の動きをコードで扱う方法について学ぶ。

インバースキネマティクス

誰かに手を握られた状態で前に引っ張られると、手が前に行き、上腕が前に行き、体が前に行き・・・と続く。この状況と同じように、体の先端の位置を決めて、その後に先端に繋がっている部分の位置を決めて・・・と外から内に向けて関節の位置を決めていく方法をインバースキネマティクスと呼ぶ。

単純化のために手や腕のようなパーツを複数のボールで表現する。この複数のボールは、自分の前にはこのボールがある、というふうにボール間の順序が決まっている。先頭のボールは手を表しており、手を引っ張ることを表現するために、マウス座標に先頭のボールが来るようにする。先頭のボール以外は、自分の前のボールまでの距離が一定間隔以内なら何もしないが、一定間隔の距離より離れてしまうとその間隔になるまで近づこうとする。これは手の先から位置を決めて、そのあとに腕の位置を・・・というふうに位置を決める。

const maxDist = 50;
let balls;

function setup() {
  createCanvas(windowWidth, windowHeight);

  balls = [];
  for (let i = 0; i < 5; i++) {
    const ball = { x: 0, y: 0 };
    balls.push(ball);
  }
}

function draw() {
  clear();

  ik(mouseX, mouseY, balls);

  balls.forEach((ball) => {
    noStroke();
    fill(240);
    circle(ball.x, ball.y, 40);
  });
}

function ik(x, y, array) {
  array[0].x = x;
  array[0].y = y;
  for (let i = 1; i < array.length; i++) {
    const prev = array[i - 1];
    const cur = array[i];
    const d = dist(prev.x, prev.y, cur.x, cur.y);
    if (maxDist < d) {
      const rad = atan2(cur.y - prev.y, cur.x - prev.x);
      cur.x = prev.x + cos(rad) * maxDist;
      cur.y = prev.y + sin(rad) * maxDist;
    }
  }
}

末尾を固定する

先ほどと似た環境だが、末尾のボールの位置は固定にするというルールを追加する。マウスと固定位置との距離が離れすぎていると、先頭がマウス座標で末尾が固定位置というルールが設定できなくなるが、この場合は末尾の位置が固定というルールを優先する。つまり、末尾は固定で、先頭のボールはなるべくマウス座標に近づこうとする動きにしたい。具体的な動作は上記実行例を確認してほしい。

この動きをコードで書きたいのだが、たとえば、末尾の位置が固定なので末尾のボールから位置を決めていくという方法が考えられる。

そのとおりに実装すると上記のようになる。マウス座標まで届かないときはボール全体がぴんとはっていいのだが、届く場合でもボール全体が一直線に並んでしまい、途中で曲がるような関節の表現ができない。

あくまでインバースキネマティクスとしての実装をしたいので、この場合はまず普通にインバースキネマティクスの計算 ik(mouseX, mouseY, balls) を行う。そうすると、マウス座標に先頭のボールが来る形になるが、末尾のボールを固定の位置におかなければならないので、この状態から、末尾のボールを固定の位置に移動する。そうすると、ボール間には位置の制約があるので、ずるずると後ろから順番に引っ張られる形になるだろう。この動きは固定の位置に向けて逆順にIKを行っていると捉えることができる。つまり、ボールが入った配列を反転させて、ik(固定位置X, 固定位置Y, 反転した配列) を実行すればいい。

まとめると、マウス座標が基準のIKの計算を行った後、配列を反転させ、固定の位置が基準のIKの計算を行うということになる。

const maxDist = 50;
let balls;

function setup() {
  createCanvas(windowWidth, windowHeight);

  balls = [];
  for (let i = 0; i < 5; i++) {
    const ball = { x: 0, y: 0 };
    balls.push(ball);
  }
}

function draw() {
  clear();

  ik(mouseX, mouseY, balls);

  const rBalls = balls.slice();
  rBalls.reverse();
  ik(width / 2, height / 2, rBalls);

  rBalls.forEach((ball) => {
    noStroke();
    fill(240);
    circle(ball.x, ball.y, 40);
  });
}

function ik(x, y, array) {
  array[0].x = x;
  array[0].y = y;
  for (let i = 1; i < array.length; i++) {
    const prev = array[i - 1];
    const cur = array[i];
    const d = dist(prev.x, prev.y, cur.x, cur.y);
    if (maxDist < d) {
      const rad = atan2(cur.y - prev.y, cur.x - prev.x);
      cur.x = prev.x + cos(rad) * maxDist;
      cur.y = prev.y + sin(rad) * maxDist;
    }
  }
}

フォワードキネマティクス

インバースキネマティクスとは逆に、体から腕に、腕から手に、というふうに内から外へ位置を決めていく方法をフォワードキネマティクスと呼ぶ。フォワードキネマティクスの動作を実装するために、これからアームという機能を作る。

アーム

上記実行例では、2つの線を用意している。ここではそのそれぞれの線をアームと呼ぶ。アーム同士は端点で繋がっており、内側のアームを移動させたり回転させたりすると、繋がっている外側のアームの位置もそれに応じて変わる。上記コードでも、動く命令を与えているのは内側部分だけで、外側は内側と繋がっている以上、結果的に合わせて移動しているという形になる。

let arm, arm2, angle;

function setup() {
  createCanvas(windowWidth, windowHeight);

  arm = Arm.create(width / 2, height / 2, 100, 0.1);
  arm2 = Arm.create(0, 0, 100, 0);
  angle = 0;
}

function draw() {
  arm.angle = Math.sin(angle) * 1.2;
  arm2.x = Arm.getEndX(arm);
  arm2.y = Arm.getEndY(arm);

  clear();
  Arm.draw(arm);
  Arm.draw(arm2);

  angle += 0.05;
}

const Arm = {
  create: function (x, y, length, angle) {
    return { x, y, length, angle };
  },

  getEndX: function (arm) {
    return arm.x + cos(arm.angle) * arm.length;
  },

  getEndY: function (arm) {
    return arm.y + sin(arm.angle) * arm.length;
  },

  draw: function (arm) {
    stroke(240);
    noFill();
    line(arm.x, arm.y, Arm.getEndX(arm), Arm.getEndY(arm));
  },
};

アームによる軌跡を描く

アームが複数繋がっている場合、外側にいくほど動きが複雑になる。この複雑さを利用して、外側のアームの位置の軌跡を描いてみよう。

const n = 60;
let arm, arm2, arm3, angle, angle2, angle3, speed, speed2, speed3, hist;

function setup() {
  createCanvas(windowWidth, windowHeight);

  arm = Arm.create(width / 2, height / 2, 100, 0.1);
  arm2 = Arm.create(0, 0, 100, 0);
  arm3 = Arm.create(0, 0, 100, 0);
  angle = 0;
  angle2 = 0;
  angle3 = 0;
  speed = 0.01;
  speed2 = 0.02;
  speed3 = 0.04;

  hist = [];
}

function draw() {
  clear();

  arm.angle = Math.sin(angle) * 5.2;
  arm2.angle += 0.05;
  arm3.angle = Math.sin(angle) * 2.9;
  arm2.x = Arm.getEndX(arm);
  arm2.y = Arm.getEndY(arm);
  arm3.x = Arm.getEndX(arm2);
  arm3.y = Arm.getEndY(arm2);

  hist.push({ x: Arm.getEndX(arm3), y: Arm.getEndY(arm3) });
  if (hist.length > n) {
    hist.shift();
  }

  let prev = hist[0];
  for (let i = 1; i < hist.length; i++) {
    const cur = hist[i];
    line(prev.x, prev.y, cur.x, cur.y);
    prev = cur;
  }
  Arm.draw(arm);
  Arm.draw(arm2);
  Arm.draw(arm3);

  angle += speed;
  angle2 += speed2;
  angle3 += speed3;
}

const Arm = {
  create: function (x, y, length, angle) {
    return { x, y, length, angle };
  },

  getEndX: function (arm) {
    return arm.x + cos(arm.angle) * arm.length;
  },

  getEndY: function (arm) {
    return arm.y + sin(arm.angle) * arm.length;
  },

  draw: function (arm) {
    stroke(240);
    noFill();
    line(arm.x, arm.y, Arm.getEndX(arm), Arm.getEndY(arm));
  },
};