Luaで書かれたマニピュレータ制御関連のプログラム部分"vectorToWorldSpace","table.sort"をPythonに直したい

受付中

回答 0

投稿

  • 評価
  • クリップ 0
  • VIEW 320

tland_p

score 7

 Pythonでマニピュレータの逆運動学解を導出するプログラムを作成

現在,ロボット関連のプログラムを作成する際にPythonでの記述が必要となり,参考サイトのLuaで書かれたプログラムをPythonで作成する作業をしています.

 発生している問題・エラーメッセージ

参考サイトInverse Kinematics for Animation

以下に逆運動学解を導出するLuaベースのプログラムを記述します.

function chain:constrain(calc, line, cf)
    local scalar = calc:Dot(line) / line.magnitude;
    local proj = scalar * line.unit;

    -- get axis that are closest
    local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))};
    local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)),  cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))};
    table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);
    table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);

    local upvec = ups[1];
    local rightvec = rights[1];

    -- get the vector from the projection to the calculated vector
    local adjust = calc - proj;
    if scalar < 0 then
        -- if we're below the cone flip the projection vector
        proj = -proj;
    end;

    -- get the 2D components
    local xaspect = adjust:Dot(rightvec);
    local yaspect = adjust:Dot(upvec);

    -- get the cross section of the cone
    local left = -(proj.magnitude * math.tan(self.left));
    local right = proj.magnitude * math.tan(self.right);
    local up = proj.magnitude * math.tan(self.up);
    local down = -(proj.magnitude * math.tan(self.down));

    -- find the quadrant
    local xbound = xaspect >= 0 and right or left;
    local ybound = yaspect >= 0 and up or down;

    local f = calc;
    -- check if in 2D point lies in the ellipse 
    local ellipse = xaspect^2/xbound^2 + yaspect^2/ybound^2;
    local inbounds = ellipse <= 1 and scalar >= 0;

    if not inbounds then
        -- get the angle of our out of ellipse point
        local a = math.atan2(yaspect, xaspect);
        -- find nearest point
        local x = xbound * math.cos(a);
        local y = ybound * math.sin(a);
        -- convert back to 3D
        f = (proj + rightvec * x + upvec * y).unit * calc.magnitude;
    end;

    -- return our final vector
    return f;
end;


が,以下の記述部分で躓いています.

local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))};
local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)),  cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))};
table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);
table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);

local upvec = ups[1];
local rightvec = rights[1];

ローカル変数ups,rightsにはそれぞれ2つのベクトルTopBottom,RightLeftが入ると思われすが,その後の記述

table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);
table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);


にてどのように処理するかがわかりません.
説明元が英語であるため,Google翻訳等で訳を出しながら作業をしていますが,関連部分の記述もあまり理解できていない状況です.

We can then use vectorToWorldSpace on that chrame to get the up. down, left, right, etc.
そのcframe上でvectorToWorldSpaceを使用して,回転を基準とした上下左右のベクトルを取得できます.

とありますが,この部分はどのようにすればPythonに実装できますでしょうか?

 試したこと

その他の記述部分は以下のようにしました.

import math
import numpy as np

class Constrain:
    def __init__(self, calc, line, cf):
        scalar = np.dot(calc, line)   # 1. point O
        proj = self.scalar * (line / np.linalg.norm(line))   # vector O

        ### ここに問題部分が入る ###

        adjust = calc - proj

        if scalar < 0:
            proj = -proj

        # get the 2D components
        xaspect = np.dot(adjust, rightvec)
        yaspect = np.dot(adjust, upvec)

        #  qj = Stan(theta j)
        left  = -(np.linalg.norm(proj) * math.tan(self.left))
        right = np.linalg.norm(proj) * math.tan(self.right)
        up    = np.linalg.norm(proj) * math.tan(self.up)
        down  = -(np.linalg.norm(proj) * math.tan(self.down))

        # find quadrant
        if xaspect >= 0:
            xbound = right
        else
            xbound = left

        if yaspect >= 0:
            ybound = up
        else 
            ybound = down

        f = calc

        # check if in 2D point lies in the ellipse
        ellipse = xaspect**2/xbound**2 + yaspect**2/ybound**2
        inbounds = False
        if elipse <= 1 && scalar >= 0:
            inbounds = True

        if inbounds == False:
            # get the angle of our out of ellipse point
            a = math.atan2(yaspect, xaspect)
            # find nearest point
            x = xbound * math.cos(a)
            y = ybound * math.sin(a)
            # convert back to 3D
            f = ()(proj + rightvec * x + upvec * y) / np.linalg.norm(proj + rightvec * x + upvec * y)) * np.linalg.norm(calc)

        return f

問題部分以外の記述は以上の通りになりますが,まだ試していない状況です.
Numpymathをインポートして計算をできるようにしました.

  • 気になる質問をクリップする

    クリップした質問は、後からいつでもマイページで確認できます。

    またクリップした質問に回答があった際、通知やメールを受け取ることができます。

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

まだ回答がついていません

15分調べてもわからないことは、teratailで質問しよう!

  • ただいまの回答率 90.21%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

同じタグがついた質問を見る

  • トップ
  • Pythonに関する質問
  • Luaで書かれたマニピュレータ制御関連のプログラム部分"vectorToWorldSpace","table.sort"をPythonに直したい