1

プログラムは、次の式を使用して順運動学でエンドエフェクタのポイントを計算します。

x = d1cos(a1) + d2cos(a1+a2)

y = d1sin(a1) + d2sin(a1+a2)

ここd1で、 は最初のジョイントd2の長さ、 は 2 番目のジョイントの長さ、 はa1最初のジョイントa2の角度、 は 2 番目のジョイントの角度です。

この式で逆運動学を計算します

ここに画像の説明を入力

ここに画像の説明を入力

したがって、フォワード キネマティクスに必要な入力を入力することで、エンド エフェクタのポイントを取得する必要があります。インバース キネマティクスのフォワード キネマティクスで見つかったポイントと同じ入力を入力することで、入力した角度をフォワード キネマティクスの入力として取得する必要があります。しかし、私はそれらを取り戻しません。ここに私のコードがあります、

'''
Created on Oct 5, 2015

@author: justin
'''
import math
def getOption():
    print('Select an option:\n')
    print('\t1) Forward Kinematics\n')
    print('\t2) Inverse Kinematics\n')
    option = input()
    try:
        option = int(option)
        if option == 1:
            fowardKinematics()
        elif option == 2:
            inverseKinematics()
        else:
            print('Not an option')
            return
    except ValueError:
        print('Not an integer/Point cannot be reached')
        return
def fowardKinematics():
    '''
    Ask user for input and computing points of end-effector 
    '''
    length1 = input('Enter length of joint 1 (a1):\n')  # Getting input from keyboard
    angle1 = input('Enter angle of joint 1 (theta1):\n')
    length2 = input('Enter length of joint 2 (a2):\n')
    angle2 = input("Enter angle of join 2 (theta2)\n")

    try:
        length1 = float(length1)  # Testing to see if user entered a number
        length2 = float(length2)  # Testing to see if user entered a number
        angle1 = float(angle1)  # Testing to see if user entered a number
        angle2 = float(angle2)  # Testing to see if user entered a number
    except ValueError:
        print('Invalid input, check your input again')
        return
    x = (length1 * math.cos(math.radians(angle1))) + (length2 * math.cos((math.radians(angle1 + angle2))))  # a1c1 + a2c12
    y = (length1 * math.sin(math.radians(angle1))) + (length2 * math.sin((math.radians(angle1 + angle2))))  # a1s1 + a2s12
    print('The position of the end-effector P(x,y) is:\n')
    print('X: ' + str(x))  # Convert x to string
    print('Y: ' + str(y))  # Convert y to string
def inverseKinematics():
    length1 = input('Enter length of joint 1 (a1):\n')
    length2 = input('Enter length of joint 2 (a2):\n')
    x = input('Enter position of X:\n')
    y = input('Enter position of Y:\n')
    try:
        length1 = float(length1)
        length2 = float(length2)
        x = float(x)
        y = float(y)
    except ValueError:
        print('Invalid input, check your input again')
        return
    # Computing angle 2 Elbow up/down
    numerator = ((length1 + length2)**2) - ((x**2) + (y**2))
    denominator = ((x**2) + (y**2)) - ((length1 - length2)**2)
    angle2UP = math.degrees(math.atan(math.sqrt(numerator/denominator)))
    angle2DOWN = angle2UP * -1

    # Angle 1 Elbow up
    numerator = (length2 * math.sin(math.radians(angle2UP)))
    denominator = ((length1 + length2) * math.cos(math.radians(angle2UP)))
    angle1UP = math.degrees(math.atan2(numerator, denominator))
    # Angle 1 Elbow down
    numerator = (length2 * math.sin(math.radians(angle2DOWN)))
    denominator = ((length1 + length2) * math.cos(math.radians(angle2DOWN)))
    angle1DOWN = math.degrees(math.atan2(numerator, denominator))
    print("Angle 1 Elbow up: " + str(angle1UP))
    print("Angle 1 Elbow down: " + str(angle1DOWN))
    print("Angle 2 Elbow up: " + str(angle2UP))
    print("Angle 2 Elbow down: " + str(angle2DOWN))
if __name__ == '__main__':
    getOption()
    pass

問題は、三角関数が導入されたときだと思います。それらのパラメーターはラジアンであると想定されており、答えは度です。どこかで私は2つを混同しています。どこにあるのかわかりません。ありがとう

4

1 に答える 1

1

残念ながら、コードまたは使用している方程式のいずれかに、これにはかなりの誤りがあります。x と y が距離で、a1 と a2 が角度の場合、theta2 の方程式は意味がありません (方程式を確認するか、ソースを提供してください)。これらが d1 と d2 である必要がある場合でも、この式では、異なる次元 (長さ ^4 と長さ ^2) を持つ 2 つの量を減算する必要があります。

次に、与えられた式を評価しない実装を確認してください。

ラジアン/度に関する私のアドバイスは、全体でラジアンを使用することです。必要に応じて角度を度で受け入れますが、すぐに計算のためにラジアンに変換し、角度の結果を度に変換して出力します。

さらにアドバイス：

  • print を使用して出力するために float を文字列にキャストする必要はありません。使用するだけprint('x: ', x)です。

  • 変数には、数式で表す記号と同じ名前を付けます。これにより、デバッグが容易になります (方程式が正しければ、そうなるでしょう)。

それが役立つことを願っています。

于 2015-10-07T14:49:38.673 に答える