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

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



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


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

0 件のコメント:

コメントを投稿

人気の投稿