以前、逆運動学の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)
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__関数（コンストラクタ）の最後２つのパラメータminAngleとmaxAngleで角度を制限します。最後のmotion()関数がマウスに追従するプログラムとなります。ゆっくりマウスを動かせば制限された角度を保ちながら一応動きます（まだ不完全な部分あり）。
FABRIKの角度制限の場合：
FABRIKの場合は、backward()とforward()の２つのメソッドがあり、それぞれに角度制限の手続きを追加しておきます。先程の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が角度制限ありメソッド。
関連：
![PythonユーザのためのJupyter[実践]入門](https://images-fe.ssl-images-amazon.com/images/I/51ZeYjBnmfL._SL160_.jpg)
