Processingのbox2dで複数のポリゴンシェイプを組み合わせた場合の表示

Processingにも物理計算をしてくれるライブラリのbox2dがあります。

このライブラリはNature of code、（この教材は素晴らしいもので、０円で入手できる。）でも紹介されている（というよりそのライブラリを作ったヒトが本の作者）のだけれど、box2dは大きなライブラリなので省かれている部分も多いですね。

例えば、polygon shapeを組み合わせてbox2d上の剛体を作った場合に、用意されているサンプルでは組み合わせ順で、最後にcreateFixtureをしたフィクスチャしか表示してくれない。

これはサンプルプログラムのdisplay関数がポリゴンの組み合わせを想定していないため。

わかっているヒトを前提として書きますが、
Fixture f = body.getFixureList();
でとれるフィクスチャは最後に加えたものだけです。
そのフィクスチャの座標だけを描画してしまうので、透明な物体が出来上がってしまう。

であれば、bodyにくっついている他のフィクスチャ（ポリゴン）の座標情報を取り出して、描画したい場合にはf = f.getNext();してボディについている次のフィクスチャを指定する必要があります。ただし、ボディについているフィクスチャの数をlengthやgetSize();で取ってくることができないので、getNext();してみて、nullでないかどうか確認して取ってくる必要があります。

具体的に、二つのポリゴンを組み合わせて描画する場合のprocessing の box2dサンプルにふくまれるPolygonsのCustamShapeというファイルを公開します。
displayの中のwhile文の中身を新しく足しました。

// The Nature of Code
// 
// Spring 2011
// Box2DProcessing example

// A rectangular box
class CustomShape {

  // We need to keep track of a Body and a width and height
  Body body;

  // Constructor
  CustomShape(float x, float y) {
    // Add the box to the box2d world
    makeBody(new Vec2(x, y));
  }

  // This function removes the particle from the box2d world
  void killBody() {
    box2d.destroyBody(body);
  }

  // Is the particle ready for deletion?
  boolean done() {
    // Let's find the screen position of the particle
    Vec2 pos = box2d.getBodyPixelCoord(body);
    // Is it off the bottom of the screen?
    if (pos.y > height) {
      killBody();
      return true;
    }
    return false;
  }

  // Drawing the box
  void display() {
    // We look at each body and get its screen position
    Vec2 pos = box2d.getBodyPixelCoord(body);
    // Get its angle of rotation
    float a = body.getAngle();

    Fixture f = body.getFixtureList();
    PolygonShape ps =  (PolygonShape) f.getShape();
    
    while(true){
      ps = (PolygonShape) f.getShape();
      rectMode(CENTER);
      pushMatrix();
      translate(pos.x, pos.y);
      rotate(-a);
      fill(175);
      stroke(0);
      beginShape();
      //println(vertices.length);
      // For every vertex, convert to pixel vector
      //while(f.getNext() == null){
        for (int i = 0; i < ps.getVertexCount(); i++) {
          Vec2 v = box2d.vectorWorldToPixels(ps.getVertex(i));
          vertex(v.x, v.y);
        }
      endShape(CLOSE);
      popMatrix();
      println(f);
      //f = f.getNext();
      if(f.getNext() == null){
        break;
      }else{
        f = f.getNext();
      }
      
    } 
  }
  // This function adds the rectangle to the box2d world
  void makeBody(Vec2 center) {

    // Define a polygon (this is what we use for a rectangle)
    PolygonShape sd1 = new PolygonShape();

    Vec2[] vertices1 = new Vec2[4];
    vertices1[0] = box2d.vectorPixelsToWorld(new Vec2(0, 3.5));
    vertices1[1] = box2d.vectorPixelsToWorld(new Vec2(10, 6));
    vertices1[2] = box2d.vectorPixelsToWorld(new Vec2(15, 8));
    vertices1[3] = box2d.vectorPixelsToWorld(new Vec2(20, 30.8));

    sd1.set(vertices1, vertices1.length);
    
    PolygonShape sd2 = new PolygonShape();
    Vec2[] vertices2 = new Vec2[4];
    vertices2[0] = box2d.vectorPixelsToWorld(new Vec2(0, -56));
    vertices2[1] = box2d.vectorPixelsToWorld(new Vec2(-5, -6));
    vertices2[2] = box2d.vectorPixelsToWorld(new Vec2(-30, -8));
    vertices2[3] = box2d.vectorPixelsToWorld(new Vec2(-50, -30.8));
    sd2.set(vertices2, vertices2.length);

    // Define the body and make it from the shape
    BodyDef bd = new BodyDef();
    bd.type = BodyType.DYNAMIC;
    bd.position.set(box2d.coordPixelsToWorld(center));
    body = box2d.createBody(bd);

    body.createFixture(sd2, 1.0);
    body.createFixture(sd1, 1.0);
  }
}

Processingでbox2dやってみたいヒトは多そうだけれどこの情報があまりなかったし、この部分で引っかかってたヒトの質問にだれも答えてなかったみたいなので書きました。結構前の質問みたいだったけど、届くといいですね。

https://forum.processing.org/one/topic/jbox2d-problem-i-cannot-render-draw-all-the-polygons-that-are-attached-to-the-body-in-my-code.html

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 でなんかできたらいいなと思います。

では。

Autodesk Fusion 360 についてくるpythonに外部Packageを導入する

学生・教職員・スタートアップ企業は無料で使用できる3D CADCAMソフトであるAutodeskのFusion360 (ふとっぱら) ではビルトインのスクリプティング機能がついており、例としてpythonを利用することができます。

私の環境(macOS HIghSierra, Fusion 360 2.03800, Ultimate, 学生版)ではメニューバーのファイル→スクリプトとアドインからスクリプトの編集を押すとSpyder IDEが起動し、Fusionと紐付けられたpython 3.5 が使用可能です。

ここから

こうする

詳細はhttp://cad.vdlz.xyz/3dCad/Fusion360/Documents/API/CreatingAScriptOrAdd-In.htmlにて。

このpythonの環境はシステム固有のpythonとは別の環境になっているので、numpy、opencv、scikit-learnなどなど、鉄板のpackageをimportしても使用できません。

今回は、このFusionに固有のpython環境に外部パッケージを追加し、使用してみるところまでをやってみます。

さて突然ですが、これは何でしょうか。

棒ですか？
いえ、これはなんと二自由度を有するアームロボットです。
根本、それから、中間の関節にモーターが入っています。
（実際にはアセンブリ機能で回転ジョイントを入れています。）

Fusionでpythonプログラムを動かすサンプルとして、このアームを作成し、動かします。
まずは、それぞれの関節の角度をプログラム上で決定し、Animateさせてみます。

サンプル1 回転ジョイントの角度をプログラムで指定する

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

def run(context):
    ui = None
    try:
        app = adsk.core.Application.get()
        ui  = app.userInterface
        
        des = adsk.fusion.Design.cast(app.activeProduct)
        root = des.rootComponent        
        
        arm = root.occurrences.itemByName('armtest v3:1')
        
        armComp = arm.component
        
        motorJoint1 = armComp.joints.itemByName('motor1')
        motorJoint2 = armComp.joints.itemByName('moter2')
        
        motor1 = motorJoint1.createForAssemblyContext(arm)
        motor2 = motorJoint2.createForAssemblyContext(arm)
        
        revJointMotion1 = motor1.jointMotion
        revJointMotion2 = motor2.jointMotion
        
        # Drive the joints.
        for k in range(0,2,1):
            for j in range(0,101,1):
                revJointMotion1.rotationValue = j*(math.pi/180.0)
                revJointMotion2.rotationValue = (90-j*2)* (math.pi/180.0)
                adsk.doEvents()
            if j>=100:               
                for j in reversed(range(0,101,1)):
                     revJointMotion1.rotationValue = j * (math.pi/180.0)
                     revJointMotion2.rotationValue = (90-j*2) * (math.pi/180.0)
                     adsk.doEvents()
    except:
        if ui:
            ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))
}

このプログラムの動作を簡単に説明すると
まず、①現在開いているCADプロジェクトのファイルからオブジェクト（ボディ）を識別する情報を入力し②ボディの中に含まれるジョイントの情報を入力し、③ジョイントを回転させるためのインスタンスを生成します。その後、それぞれの軸のインスタンスを利用して回転角度をプログラムから操作します。

実行してみましょう。
わーお。動きました。でも単純な反復運動ですね。。。

せっかくプログラムで動かしているのだから、もっとランダム性を追加し、生き物のように動かしてみたいと思いませんか？

そういった要素に適しているのはパーリンノイズというノイズ生成アルゴリズムです。
パーリンノイズはCGエフェクトに使用されるプロシージャルテクスチャを生成するため、第一作目の映画Tronにも関わっているKen Perlinが開発しました。
Ref
The Book of Shaders: Noise
パーリンノイズを理解する | プログラミング | POSTD

pythonではpackageを使うだけでこのノイズ生成関数を使用することができます。
noise 1.2.2 : Python Package Index

今回は、このnoise packageをimportし、各軸の角度をパーリンノイズを利用してまるで生き物のような動きをさせてみます。

本題です。

なぜか（ふつうに？）Fusionに固有のpythonでは、pipをインストールしても使用できませんでした。pipが使用できるならば、　pip install noise　で終わりです。

しかし、できないので手動でビルドしてインストールまで持っていきます。
まず、Fusionがどこにpythonの実体をおいているかを把握します。
私の環境では
/Users/USERNAME/Library/Application\ Support/Autodesk/webdeploy/shared/PYTHON/3.5.3c/MAC64/Python.framework/Versions/3.5/binにありました。

手順
１．先ほど示したURL or GitHubからnoiseのソースコード一式を入手する。
２．デスクトップ等に置いてみる。
３．pythonが実行できるターミナルを起動する。
４．noiseディレクトリ内部に移動。

その中で
sudo /Users/USERNAME/Library/Application\ Support/Autodesk/webdeploy/shared/PYTHON/3.5.3c/MAC64/Python.framework/Versions/3.5/bin/python setup.py build
次に

sudo /Users/USERNAME/Library/Application\ Support/Autodesk/webdeploy/shared/PYTHON/3.5.3c/MAC64/Python.framework/Versions/3.5/bin/python setup.py install
すると、

/Users/USERNAME/Library/Application\ Support/Autodesk/webdeploy/shared/PYTHON/3.5.3c/MAC64/Python.framework/Versions/3.5/lib/python3.5/site-packages/にnoise-1.2.2-py3.5-macosx-10.6-intel.egg
↑こんなようなのが生成されます。

これで良しです。
私は色々ハマってしまったので、
/Users/USERNAME/Library/Application\ Support/Autodesk/webdeploy/production/2a93884b46c5eedb87c1f008a9d77b138ef905db/Api/Python/packages/に入れなきゃいけないのではないかー。とかなんとかpipを使えるようにできないかー。
などやってしまったのですが、やり方がわかれば10分もかからずに終わるはずです。

それではおもむろにFusionを再起度しましょう。
そして、スクリプトとアドインからSpyderを起動しましょう。
先ほど示した、軸の角度をfor文で変えていくスクリプトにnoiseを追加すると

サンプル2 パーリンノイズでロボットを動かす

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

def run(context):
    ui = None
    try:    
        
        app = adsk.core.Application.get()
        ui  = app.userInterface
        
        des = adsk.fusion.Design.cast(app.activeProduct)
        root = des.rootComponent        
        
        arm = root.occurrences.itemByName('armtest v3:1')
        
        armComp = arm.component
        
        motorJoint1 = armComp.joints.itemByName('motor1')
        motorJoint2 = armComp.joints.itemByName('moter2')
        
        motor1 = motorJoint1.createForAssemblyContext(arm)
        motor2 = motorJoint2.createForAssemblyContext(arm)
        
        revJointMotion1 = motor1.jointMotion
        revJointMotion2 = motor2.jointMotion
        
        # Drive the joints.
        xt = random.random()*1000
        yt = 0
        for k in range(0,4,1):
            for j in range(0,101,1):
                i1 = noise.pnoise1(xt)
                i1 = (i1+1)*90
                i2 = noise.pnoise1(yt)
                i2 = (i2+1)*90
                xt += 0.01
                yt += 0.01
                revJointMotion1.rotationValue = i1*(math.pi/180.0)
                #revJointMotion2.rotationValue = (90-i2*2)* (math.pi/180.0)
                revJointMotion2.rotationValue = i2* (math.pi/180.0)
                adsk.doEvents()
        if ui:
            ui.messageBox('Failed:\n{}'.format(traceback.format_exc()))

上記のようにすると、パーリンノイズアルゴリズムで生成した乱数を基に各関節の角度を指定することができます。

これだけ簡単に3D Viewに反映させることができるなら、ものすごく役立ちますね。
pythonで運動学を解くプログラムを作って、Fusionで動かしてデバッグ→実機へ
みたいなのが便利そうです。

ほかのpackage、numpyなども追加することができています。
パッケージ同士の依存関係などあるので方法は少し複雑になりますが。