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

では。