ページを更新する

Dobot Magician 逆運動学 2020/06/28 ～ 2020/11/17

・2020/11/17 ほぼ正しいと思われる角度範囲を追加

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

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

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

ほぼこれ↓が正解のようです。(2020/11/17 追記）
・主軸　　：(J1 > -125) and (J1 < 125)
・アーム１：(J2 > -5) and (J2 < 90)
・アーム２：(J3 > -15) and (J3 < 90)
・アーム１とアーム２で作成される三角形の他の一辺の長さ：(dist2 <= 268.6);
　※アーム１の長さ：135.0、アーム２の長さ：146.74 として

逆運動学で X, Y, Z から逆算すると誤差が含まれるので、
プレアラーム的に使う場合は、範囲を小さめにします。0.05 程度？

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

// 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; // アーム１の辺の長さ
  L3 := 147.0; // アーム２の辺の長さ
  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);
    // アーム１、２の底辺の長さ
    ld := sqrt(l * l + (z - L1) * (z - L1));
    if (ld > 0) and (ld < (L2 + L3)) then begin
      // アーム基点から先端への仰角
      // アーム１、アーム２底辺の三角形の基点側の内角
      phi := arctan2((z - L1), l);

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

      // アーム２のアーム１からの角度
      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;
      // アーム１の角度（垂直で 0 度、前に傾斜で＋方向）
      // Dobot の場合 -5.0 ～ +84.0 まで
      J2 := 90.0 - A2;
      // アーム２の角度（水平で 0 度、前方に下がる方向が＋）
      // Dobot の場合、 -15.0 ～ +75.0 まで
      J3 := 180.0 - A2 - A3;
      result := true;
    end;
  end;
end;

// 可動範囲内にあるか (2020/11/17 値を変更)
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 := 
       (J1 > -125)  and (J2 < 125) and   // 2020/11/17 追加
       (J2 > -4.99) and (J2 < 89.9) and 
       (J3 > -14.9) and (J3 < 89.9) and
       (dist2 <= 268.6);
    // 268 はアームが最大に伸びた時の長さ（268 ～ 272mm）一直線にはならないため、制限が必要
  end;
end;