Autodesk Fusion360で2DOFアームの逆運動学

前回の記事ではFusion360に外部パッケージを読み込める形にした上で、2 degree of fredom (DOF)のアームの各関節角度をPythonにて決定し、その姿勢をFusion360に表示する。というのをやりました。

今回は一歩進め、2DOFアームの先端(end effector)を二次元空間上の一点に決め、そこから二つの関節の角度を算出する。というのをやります。

2DOFアームの逆運動学は過去の記事に解法を載せているので参考にして下さい。

それではプログラム例です。

import adsk.core, adsk.fusion, adsk.cam, traceback
import math

def run(context):
    ui = None
    try:
        app = adsk.core.Application.get()
        ui  = app.userInterface
        des = adsk.fusion.Design.cast(app.activeProduct)
        root = des.rootComponent  
        # Get the two occurrences in the root.  These are two
        # instances of the same component.
        arm = root.occurrences.itemByName('armtest v3:1')
        
        # Get the component referenced by the occurrences.
        armComp = arm.component
        
        # Get the joint in the piston component.  It will be in 
        # the context of the piston component.
        motorJoint1 = armComp.joints.itemByName('motor1')
        motorJoint2 = armComp.joints.itemByName('moter2')
        
        # Create proxies of the joint for each occurrence.
        motor1 = motorJoint1.createForAssemblyContext(arm)
        motor2 = motorJoint2.createForAssemblyContext(arm)
        
        # Get the RevoluteJointMotion object from each joint.
        revJointMotion1 = motor1.jointMotion
        revJointMotion2 = motor2.jointMotion
        l1 = 90
        l2 = 90
        r = 20
        for i in range(0, 360, 1):
            x = -50+r*math.sin(i*math.pi/180)
            y = 50+r*math.cos(i*math.pi/180)
            angle1_ = math.acos((math.pow(l1,2)-math.pow(l2,2)+(math.pow(x,2)+math.pow(y,2)))/((2*l1)*math.sqrt(math.pow(x,2)+math.pow(y,2))))
            angle1 = math.atan2(y,x)-angle1_
            angle2_ = math.acos((math.pow(l2,2)-math.pow(l1,2)+(math.pow(x,2)+math.pow(y,2)))/((2*l2)*math.sqrt(math.pow(x,2)+math.pow(y,2))))
            angle2 = angle1_ + angle2_
            the1 = angle1*math.pi/180
            the2 = angle2*math.pi/180
            revJointMotion1.rotationValue = angle1
            revJointMotion2.rotationValue = angle2
            print(angle1)
            adsk.doEvents()
    except:
        if ui:
            ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))

このプログラムを実行すると、Fusion上でx = 50 mm , y = 50 mmの点を中心として、半径r=20 mmの円周上をアーム先端がなぞるように移動します。各アームの長さはl=90 mmです。このような動きを2DOFアームにさせるためには、各関節の角度を同時に変化させる必要があるので、手動のモーションスタディ機能などでは表現できないと思います。（試してないけど）

Fusion360で表示する利点は、Fusion上の単位系がメートル記法であるために、実際にこのアームを作った時の動きと全く同様の動き方として見える点にあります。Processingなどで表示した場合では単位系がピクセルになってしまうため、どこかで単位換算をしなければいけません。

実際にこのプログラムを動作させると、

のように表示されます。非常にわかりやすい形で、逆運動学の数値計算シミュレーションの結果が得られます。

実際にこの結果を受けて、アームを作る場合にはFusion上の設計図と同じように作れば、いとも簡単に現実世界にアームを登場させることができるのです。

2DOFアームでは二次元平面上を動かすという観点において、冗長自由度が存在しないので計算x,yが決まり、かつ、その値がアームの可動範囲内に収まっていればアーム姿勢は一意に決まります。

x, yの値を関数で決定すれば良いので、

こんな感じに、ハートを描かせることもできます。２月なので。
使用した関数はHeart Curve→http://mathworld.wolfram.com/HeartCurve.html

この動画では、アーム先端に取り付けた赤色のしるしをOpenCVで認識させて、先端の軌跡を表示させるようにしています。

次は3DOFへの拡張、もしくはFusion + Python + Arduino でなんかできたらいいなと思います。

では。