Dobot Magician 逆運動学 2020/06/28

Dobot Magician の X, Y, Z の座標値からアームの角度が分かるということで、計算してみました。
参考にしたサイトは、こちら
「2リンクモデルの逆運動学を求める!順運動学の式から算出する方法」
↑Dobot M1 の左右アームの可動範囲取得にも使えますね。

実際のコードはこちらの Arduino コードをそのまま使わせていただきました。感謝いたします。
「Arduino UNO でロボットアームを堪能 ーロボットアーム自作への道4ー」

Dobot Magician の正確なアーム角度の範囲は公開されていませんので、
実際にアームを動かしてエラーを出しながら大まかな角度範囲を調べ、
それを元に制限をかけると下のような画像のような可動範囲になります。
特に下側、内側の可動範囲は動かし方によって微妙に変わるようで、
どの角度範囲を採用するか、悩ましいところです。

安全なのは左端ですが、可動域が狭くなります。ギリギリを狙うと、左から2番目になります。


// Dobot Magician アーム角度を取得
// x, y, z : エンドエフェクタの位置
// A1, A2, A3 : 主軸の回転角度、アーム 1 の傾斜角度, アーム 2 の角度(アーム 1 からの傾斜角度)
// J1, J2, J3 : 主軸の回転角度、アーム 1 の角度(垂直で 0)アーム 2 の傾斜角度(水平で 0)
function dobotArmAngle(x, y, z: double; var A1, A2, A3, J1, J2, J3 : double): boolean;
var
  th1, th2, th3 : double;
  phi, l, ld : double;
  L1, L2, L3, L4 : double;
begin
  L1 := 0.0;   // アーム基点の高さ(Dobot Magician の場合: 0)
  L2 := 135.0; // アーム1の辺の長さ
  L3 := 147.0; // アーム2の辺の長さ
  L4 := 60.0;  // エンドエフェクタの突き出し部分(必ず水平である)
  result := false;
  // X-Y平面上のアーム部分の長さ
  l := sqrt(x * x + y * y) - L4 ;
  if (l > L4) then begin
    // X-Y 平面上のアームの角度
    // Dobot の場合、-125~ +125 度であること
    th1 := arctan2(y, x);
    // アーム1、2の底辺の長さ
    ld := sqrt(l * l + (z - L1) * (z - L1));
    if (ld > 0) and (ld < (L2 + L3)) then begin
      // アーム基点から先端への仰角
      // アーム1、アーム2底辺の三角形の基点側の内角
      phi := arctan2((z - L1), l);

      // アーム1の角度
      th2 := phi + arccos((ld * ld + L2 * L2 - L3 * L3) / (2.0 * ld * L2));

      // アーム2のアーム1からの角度
      th3 := arcsin((ld * ld - L2 * L2 - L3 * L3) / (2.0 * L2 * L3)) + PI / 2.0;
      // ラジアン->デグリ換算
      A1 := th1 * 180.0 / PI;
      A2 := th2 * 180.0 / PI ;
      A3 := th3 * 180.0 / PI ;

      // Dobot Arm の角度表示に変換
      J1 := A1;
      // アーム1の角度(垂直で 0 度、前に傾斜で+方向)
      // Dobot の場合 -5.0 ~ +84.0 まで
      J2 := 90.0 - A2;
      // アーム2の角度(水平で 0 度、前方に下がる方向が+)
      // Dobot の場合、 -15.0 ~ +75.0 まで
      J3 := 180.0 - A2 - A3;
      result := true;
    end;
  end;
end;

// 可動範囲内にあるか
function dobotCheckXYZ(x, y, z : double): boolean;
var
  A1, A2, A3, J1, J2, J3 : double;
  dist1, dist2 : double;
begin
  result := false;
  if dobotArmAngle(x, y, z, A1, A2, A3, J1, J2, J3) then begin
    dist1 := getDistance(x, y);
    dist2 := getDistance(dist1 - 60.0, z);
    // (J2 < 89.00) : 大きくすると、下側に(初期値:83.99)
    // (J3 < 75.00) : 大きくすると、中心方向に(初期値:75.00)
    result := (J2 > -4.99) and (J2 < 89.00) and (J3 > -14.9) and (J3 < 75.00) and (dist2 <= 268);
    // 268 はアームが最大に伸びた時の長さ(268 ~ 272mm)一直線にはならないため、制限が必要
  end;
end;