grbl1.1+Arduino CNCシールドV3.5+bCNCを使用中。
BluetoothモジュールおよびbCNCのPendant機能でスマホからもワイヤレス操作可能。
その他、電子工作・プログラミング、機械学習などもやっています。
MacとUbuntuを使用。

CNCマシン全般について:
国内レーザー加工機と中国製レーザー加工機の比較
中国製レーザーダイオードについて
CNCミリングマシンとCNCルーターマシンいろいろ
その他:
利用例や付加機能など:
CNCルーター関係:



*CNCマシンの制作記録は2016/04/10〜の投稿に書いてあります。


ラベル FABRIK の投稿を表示しています。 すべての投稿を表示
ラベル FABRIK の投稿を表示しています。 すべての投稿を表示

2021年4月30日金曜日

IK(逆運動学):アーム可動域制限(角度制限)CCDとFABRIKの場合

以前、逆運動学のFABRIKとCCDを実装しましたが、各アームの可動域が無制限だったので今回は角度制限を追加してみました。

環境:Python3.8.5、Jupyter Notebook


小型サーボの場合、通常0〜180度程度の可動域しかないので、同じような条件にしてみました。設定変数によって可動域は変えられるようにしてます。

上図(3リンクの場合):

  • Link1は地面に対して鉛直方向を90度、Joint1を回転軸として0〜180度に設定(絶対角度)
  • Link2の可動域はLink1に対してJoint2を回転軸として-90〜90度に設定(相対角度)
  • Link3の可動域はLink2に対してJoint3を回転軸として-90〜90度に設定(相対角度)
それぞれのJointにおいてminAngleとmaxAngleを上記条件で設定しておき、例えば角度計算においてmaxAngle以上の角度が得られた場合は最大角度をmaxAngleになるように抑制します。要はこれまでのFABRIKやCCDの1ループの通常計算のあとに、その都度角度制限の補正を加えるという方法です。

CCDの角度制限の場合:

  • まずLinkの角度を-180〜180度に変換するconvTheta()を用意し角度表現を統一しておく
  • Armクラス内コンストラクタにminAngleとmaxAngleのパラメータを追加(デフォルト値を設定)
  • Armクラス内にangleLimit()メソッドを追加して角度制限する
def convTheta(theta):
    theta = theta % tau
    if theta > pi:
        theta = theta - tau
    return theta
    
class Arm:
    def __init__(self, ax, ay, length, angle, minAngle=-pi/2, maxAngle=pi/2):
        self.ax = ax
        self.ay = ay
        self.length = length
        self.angle = convTheta(angle)
        self.bx = self.ax + self.length * cos(self.angle)
        self.by = self.ay + self.length * sin(self.angle)
        self.minAngle = convTheta(minAngle)
        self.maxAngle = convTheta(maxAngle)
    
    def angleLimit(self, prevTheta, newTheta):
        theta = convTheta(newTheta - prevTheta)
        if theta < self.minAngle:
            theta = self.minAngle
        elif theta > self.maxAngle:
            theta = self.maxAngle
        self.angle = theta + prevTheta
        self.bx = self.ax + self.length * cos(self.angle)
        self.by = self.ay + self.length * sin(self.angle)

メソッドangleLimit()の引数prevThetaは一つ手前のLinkの角度、newThetaは操作後の角度。self.angleの角度を含め、すべて絶対座標上での角度として計算します。
newThetaからprevThetaを差し引いた角度thetaがself.minAngle以下ならself.minAngleのまま(maxAngleも同様に計算)。最終的に補正された角度self.angleによって、self.bxとself.by(各LinkのEnd-Effector寄りの端点)を更新するという手順になってます。
角度制限のデフォルト値は-pi/2〜pi/2(-90〜90度)に設定してあるので、無記入ならデフォルト値が適用されます。



上図:比較(3リンクアームCCDの場合):
CCD(青)が角度制限なしの計算方法、CCD_AL(ピンク)が角度制限ありの計算方法、赤いx印が目標座標。
角度制限なし(青)の方は、Link2とLink3がもう少しで重なりそうになっていますが、角度制限あり(ピンク)の方では、一つ手前のLinkに対して90度以上回転しないように設定してあるため、このような状況のときにはそれぞれのLinkは90度を保ったまま動こうとします。
角度制限ありの場合は、制限されている分、目標座標に到達できないこともあります。暫定的な角度制限アルゴリズムなので、まだ不完全な部分もあります。

上図:比較(4リンクアームCCDの場合):
角度制限なし(青)のほうでは、Linkが自在に動くためLink2とLink4が交差していますが、角度制限あり(ピンク)のほうは最大角度90度を保ったまま目標座標に到達しています。単に90度以上回転しないようにしているだけなので、Link数を増やせば角度制限ありのほうでも交差することはあります。しかしながら、角度制限なしに比べれば、より現実的な動きに近づいています。


CCD_AngleLimitのコード:
__init__関数(コンストラクタ)の最後2つのパラメータminAngleとmaxAngleで角度を制限します。最後のmotion()関数がマウスに追従するプログラムとなります。ゆっくりマウスを動かせば制限された角度を保ちながら一応動きます(まだ不完全な部分あり)。




FABRIKの角度制限の場合:

FABRIKの場合は、backward()とforward()の2つのメソッドがあり、それぞれに角度制限の手続きを追加しておきます。先程のCCDと同様にconvTheta()で角度を-180〜180度に変換しておきます。


以下が角度制限なしのbackward()メソッド。
def backward(self, tx, ty):
    theta = np.arctan2(ty - self.ay, tx - self.ax)
    self.bx = tx
    self.by = ty
    self.ax = tx - self.length * cos(theta)
    self.ay = ty - self.length * sin(theta)
    self.angle = convTheta(theta)
  
そして以下が角度制限ありのメソッド。
def backward2(self, tx, ty, prevTheta):
    theta = convTheta(np.arctan2(ty - self.ay, tx - self.ax) - prevTheta)
    if theta < self.minAngle:
        theta = self.minAngle
    elif theta > self.maxAngle:
        theta = self.maxAngle
    self.angle = convTheta(theta + prevTheta)
    self.bx = tx
    self.by = ty
    self.ax = self.bx - self.length * cos(self.angle)
    self.ay = self.by - self.length * sin(self.angle)
backward/backward2の場合は、End-Effector側から各Linkを回転移動していくため、各Linkの回転軸はEnd-Effector寄りの端点(self.bx,self.by)となり、ベース寄りの端点座標(ax,ay)が角度制限によって補正されます。その逆で、forward2の場合は(self.ax,self.ay)が回転軸となり、(self.bx,self.by)の座標が補正されます。backward2の引数prevThetaは一つ手前のLinkの角度であり、角度制限において相対角度を計算するために必要となります。


FABRIK_AngleLimitのコード:
backward/forwardが角度制限なしメソッド、backward2/forward2が角度制限ありメソッド。


関連:
実践 ロボット制御: 基礎から動力学まで
実践 ロボット制御: 基礎から動力学まで
Posted with Buyer
prime
オーム社
売上ランキング: 77028

2021年3月25日木曜日

Inverse Kinematics 逆運動学:Backward Shift、FABRIK、CCD

 ロボットアーム(マニピュレータ)における逆運動学のアルゴリズム。

今回は以下の方法について試してみます。

  • Forward + Shift
  • FABRIK(Foward and Backward Reaching Inverse Kinematics)
  • CCD(Cyclic Coordinate Descent)
環境:
  • Python3.6
  • Jupyter Notebook


運動学の場合、ロボットアームの各関節の角度を入力するとアーム先端(End-Effector)の座標値を求めることができます。
逆運動学の場合、End-Effector(アーム先端部)の座標値を入力すると各関節の角度が求められます。
上記の方法以外にヤコビ行列を用いた方法がありますが、それは次回へ

Backward Shift:

この方法は、解析的に各関節の角度を求めるというより、図形的に回転と移動を繰り返すことで徐々に目標の座標に近似させていきます。Baseに近いほうからLink1、Link2、Link3という順番にしておき、Backwardの名前の通り、Link3から回転移動処理を始め、次にLink2、そしてLink1と操作を続けていきます。


Chart1:
    アーム先端(End-Effector)の根元の関節Joint3から目標座標Target3に線を引いて角度θ3を得る。
    Joint3を軸にLink3をθ3回転させる。
Chart2:
    回転させたのち、End-EffectorとTarget3が一致するようにLink3をd3だけ平行移動する。
Chart3:
    Link2とLink3の間に隙間ができるが、これでLink3の操作は一旦終了。

次はLink2の回転移動操作。目標座標はLink3下端(Target2)。

Chart4:
    Joint2からTarget2へ向けて線を引き角度θ2を得る。
    Joint2を軸にLink2をθ2回転させる。
Chart5:
    回転させたのち、Link2上端とTarget2が一致するようにLink2をd2だけ平行移動する。
Chart6:
    Link1とLink2の間に隙間ができるが、これでLink2の操作は一旦終了。

次のLink1についても同様の手順で動かす。目標座標はLink2の下端(Target1)。

Chart7:
    Joint1からTarget1へ向けて線を引き角度θ1を得る。
    Joint1を軸にLink1をθ1回転させる。
Chart8:
    回転させたのち、Link1上端とTarget1が一致するようにLink1をd1だけ平行移動する。
Chart9:
    Link1下端とBaseの間には隙間d0ができるので、Link1下端がBaseと一致するようにd0だけ全体的に平行移動する。
Chart10:
    全体的に平行移動すると、End-Effectorと目標座標Target3との間に隙間ができる。
    Joint3からTarget3へ線を引き、これ以降はChart1に戻って同じ処理を繰り返す。
    最終的にはEnd-Effectorが目標座標Target3に限りなく近づく。
    
プログラム上では、この繰り返し処理に回数制限を設けるか、End-Effectorと目標座標との誤差が0.00001以下になったらループ処理を抜け出すかなどの設定にします。

コード:

  • リンク(Arm)のclassを用意して、長さ、両端座標、角度などを定義しておきます。
  • ならびに回転移動処理に必要なbackwardとshiftというメソッドも定義しておきます。
  • 角度はnp.arctan2()で二点の座標から求めています。
  • 変数Nを変えれば、Link数を増やすことができます。
  • Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
  • コード後半のmotion()関数で、ロボットアームがマウス座標を追従します(Jupyter上では動きはやや遅い)。



FABRIK:

この方法は、「Forward and Backward Reaching Kinematics」の略で、ForwardとBackwardの回転移動操作を繰り返して徐々に目標座標に近づいていきます。
前述したBackward ShiftのShiftさせた部分をForward(前方からの)の回転移動操作に置き換えたようなものです。
まず、End-Effectorのほうから目標座標に対して回転移動操作(Backward)し、Link1まで操作したら、今度はLink1、Link2、Link3という順番で回転移動操作(Forward)していきます。向きが変わるだけで基本的な手順は前述のChart1〜Chart8までの操作と同じです。

コード:

  • Linkのclassを用意して、長さ、両端座標、角度などを定義しておきます。
  • 回転移動処理に必要なbackwardとforwardのメソッドも定義しておきます。
  • 角度はnp.arctan2()で二点の座標から求めています。
  • 変数Nを変えれば、Link数を増やすことができます。
  • Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
  • コード後半のmotion()関数で、ロボットアームがマウス座標を追従します(Jupyter上では動きはやや遅い)。




CCD:

この方法は、2つのベクトル(回転軸から目標座標までのベクトル、回転軸からEnd-Effectorまでのベクトル)の角度の差分だけ回転させますが、Linkを回転させるというよりも、回転軸以降にあるJointの座標を回転移動させると言ったほうがいいでしょう。回転軸はJoint3、Joint2、Joint1という順番で移行し、それを繰り返します。


Chart1:
    まずEnd-Effectorに近いJoint3を回転軸とする。
    Joint3からTargetまでのベクトル、Joint3からEnd-Effectorまでのベクトル間の角度θ3を求める。
    回転軸となるJoint3以降にあるJoint(この場合End-Effectorのみ)をθ3回転させる。
Chart2:
    次の回転軸はJoint2。
    Joint2からTargetまでのベクトルとJoint2からEnd-Effectorまでのベクトル間の角度θ2を求める。
    Joint2以降のJoint(この場合、Joint3とEnd-Effector)をJoint2を中心にθ2回転させる。
Chart3:
    次の回転軸はJoint1。
    Joint1からTargetまでのベクトルとJoint1からEnd-Effectorまでのベクトルの角度θ1を求める。
    Joint1以降のJoint(この場合、Joint2、Joint3、End-Effector)をJoint1を中心にθ1回転させる。
Chart4:
    再度、回転軸はJoint3に戻り、Chart1同様の手順で回転移動させる。
    あとはこの繰り返しで徐々にEnd-EffectorはTargetに近づいていく。

コード:

  • 各Linkごとに動かすというよりも、回転軸となるJointを中心にそれ以降にあるJointの座標を回転移動させています。
  • CCD()ファンクション内の二重のforループは、最初のforループが回転軸用、次のforループが回転移動させるJoint座標用になります。
  • Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。



3つの比較:

どれも比較的簡単なアルゴリズムなので軽快に動きますが、Forward & Backward系とCCDでは少し動きに違いが出てきます。
以下は、マウス(赤x印)の動きに追従するインタラクティブなプログラム。3つの方法を重ねて同時に動かしています。
BackwardとFABRIKは同じアルゴリズムを使っているので似たような動きになります。
FABRIKは全体的に張りのある動きをしています。Backwardのほうはやや柔らかめ。
CCDはやや反応が遅く、先端に近いほうの動きとBaseに近いほうの動きに差が出て蛇行することがあります。
リンク数を多くすると、その動き方の違いも顕著になってきます。
リンク数20の場合。
マウスの動かし方にもよりますが、CCDは2つに比べるとやや不自然な形になっています。
結果、BackwardとFABRIKが反応も早く、動きも自然という感じです。それぞれの回転移動方法やコードも比較的簡単。

3つ同時のコード:

  • Backward Shift、 FABRIK、CCDの3つ方法をマウスに追従させることでインタラクティブに試すことができます。
  • 変数Nを変えればリンク数を増やすことができます。
  • Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。

これらのアルゴリズムは、CGのキャラなどを動かすときに向いているようです。角度制限がないため実際のロボットアームに使うアルゴリズムには向いていないようです。

人気の投稿