Help us understand the problem. What is going on with this article?

逆運動学解を導出するFABRIKアルゴリズムをPythonで実装する

マニピュレーターのエンドポイントから関節角度を推定する逆運動学.
今回は,FABRIK(Forward And Backward Reaching Inverse Kinematics,[著]Anrdreas Aristidou)と呼ばれる逆運動学アルゴリズムを用いて逆運動学解を導出します.
使用言語はPythonです.
文献著者HP解説(ROBLOX)

今回の目的

  • FABRIKアルゴリズムを用いて各関節の座標を求めるプログラムの作成
  • (制約のない状況なので,あらゆる関節角度をとる)

プログラム

逆運動学解を導出するために必要なプログラムを準備します.
- Initialized.py (リンク構造の初期状態)
- forward_and_backward.py(計算をするプログラム)

forward_and_backward.py

エンドポイントからベースポジションへ戻る方向をbackward
ベースポジションからエンドポイントへ進む方向をforward とします(ややこしい!)
以下の図がプログラムの流れを図示したものです.
fabrik.jpg
(a). ターゲットポジションを設定する.
(b). ターゲットポジションにエンドポイントを置き,新規エンドポイントとする.
(c). 新規ポジションi+1番目とポジションi番目との直線上にリンクの長さを合わせた場所をi番目の新規ポジションとする.
(d). ベースポジションまで計算をする.(このとき.新規ベースポジションと本来のベースポジションがズレていることが確認でき,逆方向への計算が必要となる)
(e). 基のベースポジションを新規ベースポジションとする.
(f). 求めた新規ポジションに対し,逆方向から計算する.
文献:Fig.3

この流れをターゲットポジションとエンドポイントのズレが許容範囲内に収まるまでa~fを繰り返します.
a~dまでをbackward関数,e,fをforward関数にて計算します.
ターゲットポジションがリンクの到達可能域にない場合を含め,全体の流れをsolve関数にて計算します.

import

import numpy as np
import math

計算のためにnumpymathをインポートしておきます


__init__

    def __init__(self, positions, target, linklength):
        self.P = positions
        self.t = target
        self.b = self.P[0]
        self.L = linklength
        self.n = len(self.L) 

        self.tol = tolerance 
  • self.P(配列) 各ジョイントポジション配列
  • self.t(変数) ターゲットポジション
  • self.b(変数) ベースポジション
  • self.L(配列) 各リンクの長さ配列
  • self.n(変数) リンクの配列長
  • self.tol(変数) ターゲットポジションとエンドポイントの誤差の許容値

以上の値を初期化します.

backward関数

backward関数ではエンドポイントからベースポジション方向へ新規ジョイントを決定します.

  1. ターゲットポジションを新規エンドポイントとしてセット
  2. 新規ジョイントi+1番目とジョイントi番目の距離を出す
  3. リンクの距離と2.で出した距離の商を取る
  4. 3.の結果とジョイントi番目の積,自然数1と3.の結果の差を取り新規ジョイントi+1番目との積を求め,合計値を新規ジョイントi番目とする

このようにして,backward方向での新規ジョイントが導出されます.
新規ベースポジションは,基のベースポジションと位置がズレるため,forward方向へ計算する必要があります.

    def backward(self):
        self.P[n] = self.t

        for j in range(self.n):
            i = (self.n-1)-j
            line = abs(np.linalg.norm(self.P[i+1] - self.P[i]))
            div = self.L[i] / line
            pos = (1-div) * self.P[i+1] + div * self.P[i]
            self.P[i] = pos

forward関数

forward関数ではベースポジションからエンドポイント方向へ新規ジョイントを決定します.

  1. ベースポジションを新規ベースポジションとしてセット
  2. 新規ジョイントi番目とジョイントi+1番目の距離を出す
  3. リンクの距離と2.で出した距離の商を取る
  4. 3.の結果とジョイントi+1番目の積,自然数1と3.の結果の差を取り新規ジョイントi番目との積を求め,合計値を新規ジョイントi+1番目とする

このようにしてforward方向の新規ジョイントが導出されます.

    def forward(self):
        self.P[0] = self.b

        for i in range(self.n):
            line = abs(np.linalg.norm(self.P[i+1] - self.P[i]))
            div = self.L[i] / line
            pos = (1-div) * self.P[i] + div * self.P[i+1]
            self.P[i+1] = pos

solve関数

solve関数ではbackward関数とforward関数を用いて全体的なFABRIKアルゴリズムを一連の流れで計算します.
ターゲットポジションが到達可能域外であれば,ベースポジションからターゲットポジションの直線上に到達可能な距離までのリンク構造を返します.
ターゲットポジションが到達可能域内であれば,backward関数,forward関数を用いてターゲットポジションとエンドポイントの誤差が許容範囲内に収まるまで計算を繰り返します.10回繰り返しても許容範囲を超える場合はbreakします.

    def solve(self):
        if abs(np.linalg,norm(self.t - self.P[0])) > sum(self.L):
            # target is out of reach
            for i in range(self.n):
                line = abs(np.linalg.norm(self.t - self.P[i]))
                div = self.L[i] / line
                self.P[i+1] = (1-div) * self.P[i] + div * self.t

        else:
            # target is in reach
            bcount = 0
            dif = abs(np.linalg.norm(self.P[self.n] - self.t))
            while dif > self.tol:
                self.backward()
                self.forward()
                dif = abs(np.linalg.norm(self.P[self.n] - self.t))

                bcount+=1
                if bcount > 10:
                    break

参考文献

ロボット工学(改訂版)ー機械システムのベクトル解析ー

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした