2018-05-23

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法 (その2)任意のメッシュ形状でも衝突判定したい

三角ポリゴンの集合で表現された任意形状をofxBulletで扱うためには， ofxBulletCustomShape()クラスを使います。
普通に動力学シミュレーションで使うだけなら、Exampleに倣って実行すれば問題ないかと思います。
github.com

ofxBullletCustomShapeクラスの改造

衝突判定をしたいときには、衝突グループと衝突マスクの指定を外から与えたいケースが多いのですが、標準ではそのインターフェースが用意されていませんので、ofxBulletCustomShapeクラスを魔改造してしまいましょう。

add関数に衝突グループと衝突マスク指定の追加(他のプリミティブクラスのadd関数も同様に引数付きの呼び出しを追加すると良さそうですね。)
addMesh関数に重心指定を追加（標準では頂点位置の平均位置になってしまう。)
draw関数でmeshを描画、ただしメモリを無駄に食うので注意。

ofxBulletCustomShape.hのクラス定義内に追加

public:
	bool addMesh(ofMesh a_mesh, ofVec3f a_localScaling, bool a_bUseConvexHull, ofVec3f a_centroid);
        void addMaterial(ofMaterial a_material);
	void add(short group, short mask);

protected:
	std::vector<ofMesh>		meshes;
        std::vector<ofMaterial>            materials;

ofxBulletCustomShape.cppに追加及び変更

//--------------------------------------------------------------
bool ofxBulletCustomShape::addMesh(ofMesh a_mesh, ofVec3f a_localScaling, bool a_bUseConvexHull, ofVec3f a_centroid) {
	if (a_mesh.getMode() != OF_PRIMITIVE_TRIANGLES) {
		ofLog(OF_LOG_ERROR, "ofxBulletCustomShape :: addMesh : mesh must be set to OF_PRIMITIVE_TRIANGLES!! aborting");
		return false;
	}
	if (_bAdded == true) {
		ofLog(OF_LOG_ERROR, "ofxBulletCustomShape :: addMesh : can not call after calling add()");
		return false;
	}

	ofMesh scaled_mesh(a_mesh);
	for (size_t i(0); i < scaled_mesh.getNumVertices(); ++i) {
		scaled_mesh.setVertex(i, scaled_mesh.getVertex(i) * a_localScaling);
	}
	meshes.push_back(scaled_mesh);

	btVector3 localScaling(a_localScaling.x, a_localScaling.y, a_localScaling.z);
	auto indicies = a_mesh.getIndices();
	auto verticies = a_mesh.getVertices();

	btVector3 centroid = btVector3(a_centroid.x * a_localScaling.x,
			a_centroid.y * a_localScaling.y,
			a_centroid.z * a_localScaling.z);

	if (!a_bUseConvexHull) {
		vector<btVector3> newVerts;
		for (int i = 0; i < indicies.size(); i++) {
			btVector3 vertex(verticies[indicies[i]].x, verticies[indicies[i]].y, verticies[indicies[i]].z);
			vertex *= localScaling;
			vertex -= centroid;
			newVerts.push_back(vertex);
		}

		btConvexHullShape* convexShape = new btConvexHullShape(&(newVerts[0].getX()), newVerts.size());
		convexShape->setMargin(0.01f);
		shapes.push_back(convexShape);
		centroids.push_back( ofVec3f(centroid.getX(), centroid.getY(), centroid.getZ()) );
	}
	else {
		// HULL Building code from example ConvexDecompositionDemo.cpp //
		btTriangleMesh* trimesh = new btTriangleMesh();

		for (int i = 0; i < indicies.size() / 3; i++) {
			int index0 = indicies[i * 3];
			int index1 = indicies[i * 3 + 1];
			int index2 = indicies[i * 3 + 2];

			btVector3 vertex0(verticies[index0].x, verticies[index0].y, verticies[index0].z);
			btVector3 vertex1(verticies[index1].x, verticies[index1].y, verticies[index1].z);
			btVector3 vertex2(verticies[index2].x, verticies[index2].y, verticies[index2].z);

			vertex0 *= localScaling;
			vertex1 *= localScaling;
			vertex2 *= localScaling;

			trimesh->addTriangle(vertex0, vertex1, vertex2);
		}

		//cout << "ofxBulletCustomShape :: addMesh : input triangles = " << trimesh->getNumTriangles() << endl;
		//cout << "ofxBulletCustomShape :: addMesh : input indicies = " << indicies.size() << endl;
		//cout << "ofxBulletCustomShape :: addMesh : input verticies = " << verticies.size() << endl;

		btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);

		//create a hull approximation
		btShapeHull* hull = new btShapeHull(tmpConvexShape);
		btScalar margin = tmpConvexShape->getMargin();
		hull->buildHull(margin);
		tmpConvexShape->setUserPointer(hull);

		centroid = btVector3(a_centroid.x * a_localScaling.x,
				a_centroid.y * a_localScaling.y,
				a_centroid.z * a_localScaling.z);

		//printf("ofxBulletCustomShape :: addMesh : new hull numTriangles = %d\n", hull->numTriangles());
		//printf("ofxBulletCustomShape :: addMesh : new hull numIndices = %d\n", hull->numIndices());
		//printf("ofxBulletCustomShape :: addMesh : new hull numVertices = %d\n", hull->numVertices());

		btConvexHullShape* convexShape = new btConvexHullShape();
		for (int i = 0; i<hull->numVertices(); i++) {
			convexShape->addPoint(hull->getVertexPointer()[i] - centroid);
		}

		delete tmpConvexShape;
		delete hull;

		shapes.push_back(convexShape);
		centroids.push_back(ofVec3f(centroid.getX(), centroid.getY(), centroid.getZ()));
	}
	return true;
}

//--------------------------------------------------------------
void ofxBulletCustomShape::add(short group, short mask) {
	_bAdded = true;
	btTransform trans;
	trans.setIdentity();

	for (int i = 0; i < centroids.size(); i++) {
		_centroid += centroids[i];
	}
	if (centroids.size() > 0)
		_centroid /= (float)centroids.size();
	btVector3 shiftCentroid;
	for (int i = 0; i < shapes.size(); i++) {
		shiftCentroid = btVector3(centroids[i].x, centroids[i].y, centroids[i].z);
		shiftCentroid -= btVector3(_centroid.x, _centroid.y, _centroid.z);
		trans.setOrigin((shiftCentroid));
		((btCompoundShape*)_shape)->addChildShape(trans, shapes[i]);
	}
	_rigidBody = ofGetBtRigidBodyFromCollisionShape(_shape, _startTrans, _mass);
	setCreated(_rigidBody);
	createInternalUserData();
	_world->addRigidBody(_rigidBody, group, mask); ///// changed
	setProperties(.4, .75);
	setDamping(.25);
}

//--------------------------------------------------------------
void ofxBulletCustomShape::addMaterial(ofMaterial a_material) {
	materials.push_back(a_material);
}

//--------------------------------------------------------------
void ofxBulletCustomShape::draw() {
	if (meshes.size()) {
		ofPushMatrix();
		transformGL();
		for (size_t i(0); i < meshes.size(); ++i) {
			if (i < materials.size()) {
				materials[i].begin();
				meshes[i].draw();
				materials[i].end();
			}
			else
				meshes[i].draw();
		}
		restoreTransformGL();
		ofPopMatrix();
	}
}

呼び出し方

呼び出しのサンプルは、前回のページとの差分を部分的に抽出すると
モデル作成

	shapes.push_back(new ofxBulletCustomShape());
	for (int i = 0; i < model.getNumMeshes(); i++) {
		((ofxBulletCustomShape*)shapes.back())->addMesh(model.getMesh(i), ofVec3f(1.f, 1.f, 1.f), false, ofVec3f(0.f, 0.f, 0.f));

		ofxAssimpMeshHelper & meshHelper = assimpModel.getMeshHelper(i);
		logos[i]->addMaterial(meshHelper.material);
	}
	((ofxBulletCustomShape*)shapes.back())->create(world.world, ofVec3f(), 0.1);
	shapes.back()->add(1, 7);

位置の更新

	btTransform trans = ofGetBtTransform( model.getModelMatrix().getTranslation() + model_mat.getTranslation(),  model.getModelMatrix().getRotate() * model_mat.getRotate());
	shapes[id]->getRigidBody()->setWorldTransform(trans);
	shapes[id]->getRigidBody()->getMotionState()->setWorldTransform(trans);

	world.update(0.f, 1);

Assimpの座標への変換がややこしい。addModelの際に頂点座標を変換して織り込んでしまったほうが良いかもしれない。
描画の更新

	ofEnableLighting();
	ofPushStyle();
	//world.drawDebug();
	for (size_t i(0); i < shapes.size(); i++) {
		if (bColliding[i]) {
			ofSetColor(ofColor::yellow, 50);
		}
		else {
			ofSetColor(ofColor::white, 50);
		}
		shapes[i]->draw();
	}
	ofPopStyle();

*1->addMesh(model.getMesh(i), ofVec3f(1.f, 1.f, 1.f), false, ofVec3f(0.f, 0.f, 0.f));の3番目の引数のfalseが重要。
falseだと簡易化した多面体として衝突テストを行い、trueだと生のポリゴンで衝突テストを行う。実用的な速さを求めるなら、false。精度が欲しいならtrue。

今日の本文

レイリが面白い。

レイリ第4巻 (少年チャンピオン・コミックスエクストラ)

作者: 室井大資岩明均
出版社/メーカー: 秋田書店
発売日: 2017/10/20
メディア: コミック
この商品を含むブログ (1件) を見る

*1:ofxBulletCustomShape*)shapes.back(

2018-05-21

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法

OMPLが自由に使えるようになって経路計画がお手軽で楽しいのですが、衝突判定を自分で書くのが億劫です。プリミティブな形状同士の衝突判定は一つ一つであればたいして難しくもありませんが、効率的に、さまざまな形状の、複数物体同士を、衝突するものとしないもののグループを考慮して、衝突判定する、なんていうことを考え始めると、途端に面倒くさくなります。

衝突判定ライブラリ探し

衝突判定ライブラリはすでに様々な提案があるのですが、openFrameworksで簡単に使えるということを考えるとC++がよく、なおかつ誰かがすでにaddonを作ってくれていることが望ましいということになります。
文献1に素晴らしいまとめがあります。どの程度最新のものまでをカバーしているのかわかりませんが、少なくともこの2年ぐらいのメジャーなライブラリが表にまとめられています。この中で、該当しそうなものを探すとFCLかBullet, ODE, ReactPhysics3dが良さそうだとわかります。FCLのofAddonを探してみましたが見つかりませんでした。またFCLはまだ開発途上だと本家のページに注意書きがあります。ofxODEもよさそうですが、以前使った感触として分かりにくかった記憶があるので今回は避けます。ofxBulletは動力学シミュレーションを使った派手な演出を行うために、結構有名です。動力学計算の機能は不要ですが、今回はofxBulletの衝突判定機能だけを取り出して使ってみたいと思います。

衝突判定のサンプル

物体が動かない時間が止まった状態で、こちらで与えた物体の位置姿勢で、物体同士が衝突を起こしているかどうかを判別したいと思います。サンプルとして、球、箱、円柱の3種類のプリミティブに適当な位置姿勢を与えて判定することにします。ただし、球と箱は仲がよいもの同士と仮定して、球と箱は衝突状態でも衝突を判定しないことにします。

物体の位置姿勢を強制的に更新する方法は[4]で示されています。

衝突フィルタリングの方法は、[5][6]で示されています。
worldへの登録は、shapes->add関数ではなく､フラグを付けつつ自前でやります。world.update()は静的に判定できれば良いので、時間を0、更新回数を1回にします。

大雑把なフローチャートはこんな感じで、簡単。衝突判定結果は、コールバック関数の中でフラグの更新をして返されます。
f:id:cvl-robot:20180524122228p:plain

ofApp.h

#pragma once

#include "ofMain.h"
#include "ofxBullet.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		
		ofxBulletWorldRigid	world;
		std::vector<ofxBulletRigidBody*> shapes;
		ofEasyCam cam;
		ofLight light;
		std::vector<bool> bColliding;

		void onCollision(ofxBulletCollisionData& cdata);
};

ofApp.cpp

#include "ofApp.h"

//--------------------------------------------------------------
void ofApp::onCollision(ofxBulletCollisionData& cdata) {
	for (int i = 0; i < shapes.size(); i++) {
		if(*shapes[i] == cdata) {
			bColliding[i] = true;
		}
	}
}

//--------------------------------------------------------------
void ofApp::setup(){
	ofSetFrameRate(60);
	ofSetVerticalSync(true);
	ofSetBackgroundColor(ofColor::black);

	world.setup();
	world.enableCollisionEvents();
	ofAddListener(world.COLLISION_EVENT, this, &ofApp::onCollision);
	world.setCamera(&cam);
	world.setGravity(ofVec3f(0, 0, -9.8f));

	// Collision Settings
#define BIT(x) (1<<(x))
	enum collisiontypes {
		COL_NOTHING = 0,
		COL_SPHERE = BIT(0),
		COL_BOX = BIT(1),
		COL_CAPSULE = BIT(2),
		COL_CONE = BIT(3),
		COL_CYLINDER = BIT(4)
	};

	// Objects
	shapes.push_back(new ofxBulletSphere());
	((ofxBulletSphere*)shapes.back())->init(ofBtGetSphereCollisionShape(0.8f));
	((ofxBulletSphere*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_SPHERE, COL_CAPSULE | COL_CONE | COL_CYLINDER);

	shapes.push_back(new ofxBulletBox());
	((ofxBulletBox*)shapes.back())->init(ofBtGetBoxCollisionShape(1.2f));
	((ofxBulletBox*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_BOX, COL_CAPSULE | COL_CONE | COL_CYLINDER);

#if 0
	shapes.push_back(new ofxBulletCapsule());
	((ofxBulletCapsule*)shapes.back())->init(ofBtGetCapsuleCollisionShape(0.4f, 2.2f));
	((ofxBulletCapsule*)shapes.back())->create(world.world, ofVec3f(0.f, 0.f, 0.f), 0.2f);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CAPSULE, COL_SPHERE | COL_BOX | COL_CONE | COL_CYLINDER);

	shapes.push_back(new ofxBulletCone());
	((ofxBulletCone*)shapes.back())->init(ofBtGetConeCollisionShape(0.9f, 0.6f));
	((ofxBulletCone*)shapes.back())->create(world.world);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CONE, COL_SPHERE | COL_BOX | COL_CAPSULE);
#endif

	shapes.push_back(new ofxBulletCylinder());
	((ofxBulletCylinder*)shapes.back())->init(ofBtGetCylinderCollisionShape(0.7f, 0.7f));
	((ofxBulletCylinder*)shapes.back())->create(world.world, ofVec3f(0.f, 0.f, 0.f), 0.4f);
	shapes.back()->setActivationState(DISABLE_DEACTIVATION);
	world.world->addCollisionObject(shapes.back()->getCollisionObject(), COL_CYLINDER, COL_SPHERE | COL_BOX | COL_CAPSULE);

	for (size_t i(0); i < shapes.size(); ++i) {
		bColliding.push_back(false);
	}

	// Camera and Light settings
	light.setParent(cam, true);
	cam.setFarClip(16.f);
	cam.setNearClip(0.001f);
	cam.setDistance(8.f);
	cam.setPosition(0.f, -8.f, 0.f);
	cam.lookAt(ofVec3f(0.f, 0.f, 0.f), ofVec3f(0.f, 0.f, 1.f));
}

//--------------------------------------------------------------
void ofApp::update(){
	for (size_t i(0); i < bColliding.size(); i++) {
		bColliding[i] = false;
	}
	world.update(0.f, 1);
}

//--------------------------------------------------------------
void ofApp::draw(){
	ofEnableDepthTest();
	ofEnableLighting();
	light.enable();

	cam.begin();
	ofSetColor(ofColor::white);
	for (size_t i(0); i < shapes.size(); i++) {
		ofPushStyle();
		if (bColliding[i]) ofSetColor(ofColor::yellow);
		shapes[i]->draw();
		ofPopStyle();
	}

	// draw GroundPlane
	ofDisableLighting();
	ofPushMatrix();
	ofTranslate(0.f, 0.f, 0.f);
	ofRotate(90.f, 0.f, 1.f, 0.f);
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.1f, 10, false);
	ofPopStyle();
	ofPopMatrix();

	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){
	// Updates for Shapes Position
	for (size_t i(0); i < shapes.size(); i++) {
		ofVec3f pos = ofVec3f(ofRandom(-2.f, 2.f), ofRandom(-2.f, 2.f), ofRandom(-2.f, 2.f));
		ofQuaternion quat;
		quat.makeRotate(ofVec3f(0.f, 1.f, 0.f), ofVec3f(ofRandom(-1.f, 1.f), ofRandom(-1.f, 1.f), ofRandom(-1.f, 1.f)).normalized());
		btTransform trans = ofGetBtTransform(pos, quat);
		shapes[i]->getRigidBody()->setWorldTransform(trans);
		shapes[i]->getRigidBody()->getMotionState()->setWorldTransform(trans);
	}
}

実行結果はこんな感じ。
何かキーを押すと、位置姿勢がランダムに更新されます。衝突していれば黄色、していなければ白色でプリミティブが表示されます。
f:id:cvl-robot:20180521174122p:plain
f:id:cvl-robot:20180521174128p:plain

コメントアウトしてあるCapsuleやConeは、ofxBulletUtils.hにわずかな変更を加えると使えるようになります。

static btCapsuleShape* ofBtGetCapsuleCollisionShape(float a_radius, float a_height) {
	return new btCapsuleShape(a_radius, a_height);
}

static btConeShape* ofBtGetConeCollisionShape(float a_radius, float a_height) {
	return new btConeShape(a_radius, a_height);
}

また、ofxBulletのCylinderやCapsuleのcreate関数の姿勢の指定にはバグがあります。Quaternionと指定しているのに内部でAxis-Angleとして渡しています。トラブル防止のために、Create関数では姿勢を与えないほうが無難です。

[1] Awesome Collision Detection
github.com

[2] ofxBullet
github.com

[3] Bulletについて
Bullet - Game Science Wiki

[4] Bulletで強制的に位置姿勢を更新する方法
stackoverflow.com

[5] 衝突テストを行う物同士のラベル付けの方法
Bullet Physics Japanese Manual - 06

[6] Collision Filtering
Collision Filtering - Physics Simulation Wiki

[]
2018年5月15日 – プログラミング de 落書き

今日のマンガ

星崎真紀先生の、魔法のリノベ全3巻がリアルで面白い。発行部数が少ないのか書店で見かけることはまずありませんので、ネットで注文。

魔法のリノベ： 1 (ジュールコミックス)

作者: 星崎真紀
出版社/メーカー: 双葉社
発売日: 2016/04/16
メディア: Kindle版
この商品を含むブログを見る

魔法のリノベ(2) (ジュールコミックス)

作者: 星崎真紀
出版社/メーカー: 双葉社
発売日: 2017/03/17
メディア: コミック
この商品を含むブログを見る

魔法のリノベ(3) (ジュールコミックス)

作者: 星崎真紀
出版社/メーカー: 双葉社
発売日: 2018/03/17
メディア: コミック
この商品を含むブログを見る

2018-05-10

openFrameworksでOMPLを動かしたい（その5）

また[1]を参考に、修正を加えてみます。
１．障害物を追加
２．cboundsの値を調整
３．アニメーションを追加

#pragma once

#include "ofxOMPL.h"
#include "ofMain.h"

class Scheduler : public ofThread {
public:
	unsigned long counter = 0;
	std::vector<double> durations;

	Scheduler() {
		counter = 0;
	}

	void start() {
		counter = 0;
		timer.setPeriodicEvent(durations[0] * 1000000000);
		baseElapsedTimeMills = ofGetElapsedTimeMillis();
		startThread();
	}

	void setDurations(std::vector<double>& d) {
		durations = d;
	}

	uint64_t getPeriod()
	{
		return ofGetElapsedTimeMillis() - baseElapsedTimeMills;
	}

private:
	ofTimer timer;
	void threadedFunction() {
		while (isThreadRunning()) {
			timer.waitNext();

			baseElapsedTimeMills = ofGetElapsedTimeMillis();
			counter++;
			if (counter >= durations.size()) {
				counter = 0;
			}
			timer.setPeriodicEvent(durations[counter] * 1000000000);
		}
	}
	uint64_t baseElapsedTimeMills;
};

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
		void planWithSimpleSetup();
		bool isStateValid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

		std::shared_ptr<ompl::base::RealVectorBounds> bounds;
		std::shared_ptr<ompl::base::RealVectorBounds> cbounds;
		//std::shared_ptr<ompl::geometric::SimpleSetup> ss;
		void propagate(const ompl::base::State *start, const ompl::control::Control *control, const double duration, ompl::base::State *result);
		ompl::base::PlannerStatus solved;

protected:
		ofEasyCam cam;
		ofBoxPrimitive boundsbox;
		ofBoxPrimitive obstacle;
		ofPolyline polyline;
		std::vector<ofMatrix4x4> stateAxis;
		std::vector<ofVec3f> stateAxisRaw;
		std::vector<ofVec3f> controlAxis;
		Scheduler mysch;
};

*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ofApp.h"

#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;

template<typename T>
inline T FMod(const T &x, const T &y)
{
	if (y == 0)  return x;
	return x - y*std::floor(x / y);
}
// Convert radian to [-pi,pi)
double RadToNPiPPi(const double &x)
{
	return FMod(x + PI, PI*2.0) - PI;
}

// a decomposition is only needed for SyclopRRT and SyclopEST
class MyDecomposition : public oc::GridDecomposition
{
public:
	MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
		: GridDecomposition(length, 2, bounds)
	{
	}
	void project(const ob::State* s, std::vector<double>& coord) const override
	{
		coord.resize(2);
		coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
		coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
	}

	void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
	{
		sampler->sampleUniform(s);
		s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
	}
};

ofApp* app;

bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	return app->isStateValid(si, state);
}

bool ofApp::isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	//    ob::ScopedState<ob::SE2StateSpace>
	// cast the abstract state type to the type we expect
	const auto *se2state = state->as<ob::SE2StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);

	// check validity of state defined by pos & rot
	ofVec3f low = -0.5f * obstacle.getSize() + obstacle.getPosition();
	ofVec3f high = 0.5f * obstacle.getSize() + obstacle.getPosition();
	if ((low.x <= pos->values[0] && pos->values[0] < high.x) &&
		(low.y <= pos->values[1] && pos->values[1] < high.y)) {
		return false;
	}

	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
}

void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	app->propagate(start, control, duration, result);
}

void ofApp::propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
	const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
	const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
	const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	result->as<ob::SE2StateSpace::StateType>()->setXY(
		pos[0] + ctrl[0] * duration * cos(rot),
		pos[1] + ctrl[0] * duration * sin(rot));
	result->as<ob::SE2StateSpace::StateType>()->setYaw(
		RadToNPiPPi(rot + ctrl[1] * duration));
}

void ofApp::planWithSimpleSetup()
{
	// construct the state space we are planning in
	auto space(std::make_shared<ob::SE2StateSpace>());

	// set the bounds for the R^2 part of SE(2)
	bounds = std::make_shared<ob::RealVectorBounds>(2);
	bounds->setLow(-1);
	bounds->setHigh(1);

	space->setBounds(*bounds);

	// create a control space
	auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));

	// set the bounds for the control space
	cbounds = std::make_shared<ob::RealVectorBounds>(2);
	cbounds->setLow(0.f, 0.f);
	cbounds->setHigh(0.f, 0.5f);
	cbounds->setLow(1.f, -2.f);
	cbounds->setHigh(1.f, 2.f);

	cspace->setBounds(*cbounds);

	// define a simple setup class
	oc::SimpleSetup ss(cspace);

	// set the state propagation routine
	ss.setStatePropagator(::propagate);

	// set state validity checking for this space
	ss.setStateValidityChecker(
		[&ss](const ob::State *state) { return ::isStateValid(ss.getSpaceInformation().get(), state); });

	// create a start state
	ob::ScopedState<ob::SE2StateSpace> start(space);
	start->setX(-0.5);
	start->setY(0.0);
	start->setYaw(0.0);

	// create a  goal state; use the hard way to set the elements
	ob::ScopedState<ob::SE2StateSpace> goal(space);
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
	(*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;


	// set the start and goal states
	ss.setStartAndGoalStates(start, goal, 0.05);

	//ss.setPlanner(std::make_shared<oc::PDST>(ss.getSpaceInformation()));
	//ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
	// attempt to solve the problem within one second of planning time
	solved = ss.solve(10.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen

		ss.getSolutionPath().printAsMatrix(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;

	// setups for drawings
	// bound
	std::vector<double> h = bounds->high;
	std::vector<double> l = bounds->low;
	ofVec3f high, low, vol;
	for (int i = 0; i < h.size(); i++) {
		high[i] = h[i];
		low[i] = l[i];
	}
	vol = high - low;
	boundsbox.set(vol[0], vol[1], vol[2], 1, 1, 1);
	boundsbox.setPosition((high + low) / 2.f);

	if (solved)
	{
		stateAxis.clear();
		polyline.clear();
		controlAxis.clear();
		for (int i = 0; i < ss.getSolutionPath().getStateCount(); i++) {
			std::vector<double> reals;
			ss.getStateSpace()->copyToReals(reals, ss.getSolutionPath().getState(i));
			ofMatrix4x4 mat;
			mat.setTranslation(reals[0], reals[1], 0.f);
			ofQuaternion quat;
			quat.makeRotate(ofRadToDeg(reals[2]), ofVec3f(0.f, 0.f, 1.f));
			mat.setRotate(quat);
			stateAxis.push_back(mat);
			polyline.addVertex(mat.getTranslation());

			stateAxisRaw.push_back(ofVec3f(reals[0], reals[1], reals[2]));
		}

		for (int i = 0; i < ss.getSolutionPath().getControlCount(); i++) {
			const double* c = ss.getSolutionPath().getControl(i)->as<oc::RealVectorControlSpace::ControlType>()->values;
			double duration = ss.getSolutionPath().getControlDuration(i);
			controlAxis.push_back(ofVec3f(c[0], c[1], duration));
		}
		mysch.setDurations(ss.getSolutionPath().getControlDurations());
	}
}


//--------------------------------------------------------------
void ofApp::setup() {
	app = this;
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	obstacle.setWidth(0.2f);
	obstacle.setHeight(0.8f);
	obstacle.setDepth(0.2f);
	obstacle.setPosition(-0.2f, 0.2f, 0.f);
	planWithSimpleSetup();
	mysch.start();

	cam.setNearClip(0.001f);
	cam.setTarget(boundsbox);
	cam.setDistance(boundsbox.getSize().length() * 2.f);
}

//--------------------------------------------------------------
void ofApp::update() {

}

//--------------------------------------------------------------
void ofApp::draw() {
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundsbox.drawWireframe();
	ofSetColor(ofColor::darkBlue, 66);
	obstacle.draw();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i <= mysch.counter; i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundsbox.getSize().length() / 100.f);
			ofPopMatrix();
		}

		float t = (float)mysch.getPeriod() / 1000.f;
		ofVec3f pos = stateAxisRaw[mysch.counter];
		ofVec3f ctr = controlAxis[mysch.counter];
		ofMatrix4x4 mat;
		float rad = RadToNPiPPi(pos[2] + t * ctr[1]);
		mat.setTranslation(pos[0] + ctr[0] * t * cos(pos[2]),
				pos[1] + ctr[0] * t * sin(pos[2]),
				0.f);
		ofQuaternion quat;
		quat.makeRotate(ofRadToDeg(rad), ofVec3f(0.f, 0.f, 1.f));
		mat.setRotate(quat);
		ofPushMatrix();
		ofMultMatrix(mat);
		ofDrawAxis(boundsbox.getSize().length() / 100.f);
		ofPopMatrix();
	}
	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key) {

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key) {

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button) {

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y) {

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h) {

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg) {

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo) {

}

実行結果はこんな感じ。障害物を避けてスタートとゴールを結んでいます。
アニメーションは、よく見ると時々はみ出している・・・何かを間違えているっぽい気がする。。。
f:id:cvl-robot:20180510230334g:plain

[1] Robotics/OMPL - NAIST::OnlineText 制御指令値を含むプラニング

2018-05-10

openFrameworksでOMPLを動かしたい（その4）

omplのdemo_RigidBodyPlanningWithControlsをopenFrameworksに移植していきます。

ofxOMPL.hの更新

必要なヘッダーファイルを足していきます。
ofxOMPL.h

#pragma once

// Plaease add these settings manually
// #include path example:	D:\workspace\ompl\src
//					D:\workspace\eigen-334
//					D:\workspace\boost_1_58_0
// #library path example:	D:\workspace\ompl\build\release\lib\Release
//					(D:\workspace\ompl\build\release\lib\Debug)
//					D:\workspace\boost_1_58_0\stage\x64\lib

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>

#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/planners/kpiece/KPIECE1.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/planners/est/EST.h>
#include <ompl/control/planners/syclop/SyclopRRT.h>
#include <ompl/control/planners/syclop/SyclopEST.h>
#include <ompl/control/planners/pdst/PDST.h>
#include <ompl/control/planners/syclop/GridDecomposition.h>
#include <ompl/control/SimpleSetup.h>

#include <ompl/geometric/planners/prm/PRM.h>

#if _DEBUG
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#else
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#endif

myRigidBodyPlanningWithControlsプロジェクト

main.cppはデフォルトのままなので省略。
ofApp.h

#pragma once

#include "ofxOMPL.h"
#include "ofMain.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
		void planWithSimpleSetup();
		bool isStateValid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

		std::shared_ptr<ompl::base::RealVectorBounds> bounds;
		std::shared_ptr<ompl::base::RealVectorBounds> cbounds;
		//std::shared_ptr<ompl::geometric::SimpleSetup> ss;
		ompl::base::PlannerStatus solved;
protected:
		ofEasyCam cam;
		ofBoxPrimitive boundsbox;

		ofPolyline polyline;
		std::vector<ofMatrix4x4> stateAxis;
};

ofApp.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ofApp.h"

#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;

// a decomposition is only needed for SyclopRRT and SyclopEST
class MyDecomposition : public oc::GridDecomposition
{
public:
	MyDecomposition(const int length, const ob::RealVectorBounds& bounds)
		: GridDecomposition(length, 2, bounds)
	{
	}
	void project(const ob::State* s, std::vector<double>& coord) const override
	{
		coord.resize(2);
		coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
		coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
	}

	void sampleFullState(const ob::StateSamplerPtr& sampler, const std::vector<double>& coord, ob::State* s) const override
	{
		sampler->sampleUniform(s);
		s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
	}
};

ofApp* app;

bool isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	return app->isStateValid(si, state);
}

bool ofApp::isStateValid(const oc::SpaceInformation *si, const ob::State *state)
{
	//    ob::ScopedState<ob::SE2StateSpace>
	// cast the abstract state type to the type we expect
	const auto *se2state = state->as<ob::SE2StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);

	// check validity of state defined by pos & rot


	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return si->satisfiesBounds(state) && (const void*)rot != (const void*)pos;
}

void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
{
	const auto *se2state = start->as<ob::SE2StateSpace::StateType>();
	const double* pos = se2state->as<ob::RealVectorStateSpace::StateType>(0)->values;
	const double rot = se2state->as<ob::SO2StateSpace::StateType>(1)->value;
	const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	result->as<ob::SE2StateSpace::StateType>()->setXY(
		pos[0] + ctrl[0] * duration * cos(rot),
		pos[1] + ctrl[0] * duration * sin(rot));
	result->as<ob::SE2StateSpace::StateType>()->setYaw(
		rot + ctrl[1] * duration);
}

void ofApp::planWithSimpleSetup()
{
	// construct the state space we are planning in
	auto space(std::make_shared<ob::SE2StateSpace>());

	// set the bounds for the R^2 part of SE(2)
	bounds = std::make_shared<ob::RealVectorBounds>(2);
	bounds->setLow(-1);
	bounds->setHigh(1);

	space->setBounds(*bounds);

	// create a control space
	auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));

	// set the bounds for the control space
	cbounds = std::make_shared<ob::RealVectorBounds>(2);
	cbounds->setLow(-0.3);
	cbounds->setHigh(0.3);

	cspace->setBounds(*cbounds);

	// define a simple setup class
	oc::SimpleSetup ss(cspace);

	// set the state propagation routine
	ss.setStatePropagator(propagate);

	// set state validity checking for this space
	ss.setStateValidityChecker(
		[&ss](const ob::State *state) { return ::isStateValid(ss.getSpaceInformation().get(), state); });

	// create a start state
	ob::ScopedState<ob::SE2StateSpace> start(space);
	start->setX(-0.5);
	start->setY(0.0);
	start->setYaw(0.0);

	// create a  goal state; use the hard way to set the elements
	ob::ScopedState<ob::SE2StateSpace> goal(space);
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
	(*goal)[0]->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.5;
	(*goal)[1]->as<ob::SO2StateSpace::StateType>()->value = 0.0;


	// set the start and goal states
	ss.setStartAndGoalStates(start, goal, 0.05);

	//ss.setPlanner(std::make_shared<oc::PDST>(ss.getSpaceInformation()));
	//ss.getSpaceInformation()->setMinMaxControlDuration(1,100);
	// attempt to solve the problem within one second of planning time
	solved = ss.solve(10.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen

		ss.getSolutionPath().printAsMatrix(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;

	// setups for drawings
	// bound
	std::vector<double> h = bounds->high;
	std::vector<double> l = bounds->low;
	ofVec3f high, low, vol;
	for (int i = 0; i < h.size(); i++) {
		high[i] = h[i];
		low[i] = l[i];
	}
	vol = high - low;
	boundsbox.set(vol[0], vol[1], vol[2], 1, 1, 1);
	boundsbox.setPosition((high + low) / 2.f);

	if (solved)
	{
		stateAxis.clear();
		polyline.clear();
		for (int i = 0; i < ss.getSolutionPath().getStateCount(); i++) {
			std::vector<double> reals;
			ss.getStateSpace()->copyToReals(reals, ss.getSolutionPath().getState(i));
			/*
			std::cout << reals[0] << " " << reals[1] << " " << reals[2] << " ";
			if (i >= 1 && i <= ss.getSolutionPath().getControlCount()) {
				const double* c = ss.getSolutionPath().getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
				double duration = ss.getSolutionPath().getControlDuration(i-1);
				std::cout << c[0] << " " << c[1] << " " << duration;
			}
			else {
				std::cout << 0 << " " << 0 << " " << 0;
			}
			std::cout << std::endl;
			*/
			ofMatrix4x4 mat;
			mat.setTranslation(reals[0], reals[1], 0.f);
			ofQuaternion quat;
			quat.makeRotate(ofRadToDeg(reals[2]), ofVec3f(0.f, 0.f, 1.f));
			mat.setRotate(quat);
			stateAxis.push_back(mat);
			polyline.addVertex(mat.getTranslation());
		}
	}
}


//--------------------------------------------------------------
void ofApp::setup(){
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	planWithSimpleSetup();

	cam.setNearClip(0.001f);
	cam.setTarget(boundsbox);
	cam.setDistance(boundsbox.getSize().length() * 2.f);
}

//--------------------------------------------------------------
void ofApp::update(){

}

//--------------------------------------------------------------
void ofApp::draw(){
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundsbox.drawWireframe();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i < stateAxis.size(); i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundsbox.getSize().length() / 100.f);
			ofPopMatrix();
		}
	}
	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

2次元空間で、Stateは(x, y, yaw)です。回転の単位はラジアン。制御のパラメータは、(前進速度、回転速度、期間)です。
よくわからないまま動かしてみると、こんな結果になります。
f:id:cvl-robot:20180510191815p:plain
なんかよくわからないふらふらした動きしてますね。

2018-05-10

openFrameworksでOMPLを動かしたい（その3）

障害物を追加してみましょう。参考文献１を参考に、障害物を置きます。

変更点を抜粋して説明していきます。
１．障害物オブジェクトobstacleをofApp.hで定義します。
ofAppクラスに物体を定義します。

ofBoxPrimitive obstacle;

２．isStateValid関数に判別ルーチンを追加。姿勢は考慮せず、obstacle内には進入禁止の条件を追加

bool ofApp::isStateValid(const ob::State *state)
{
	// cast the abstract state type to the type we expect
	const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

	// check validity of state defined by pos & rot
	ofVec3f low = -0.5f * obstacle.getSize() + obstacle.getPosition();
	ofVec3f high = 0.5f * obstacle.getSize() + obstacle.getPosition();
	if ((low.x <= pos->values[0] && pos->values[0] < high.x) &&
		(low.y <= pos->values[1] && pos->values[1] < high.y) &&
		(low.z <= pos->values[2] && pos->values[2] < high.z)) {
		return false;
	}

	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return (const void*)rot != (const void*)pos;
}

３．obstacleの形状定義をofApp::setupに追加。

void ofApp::setup(){
	app = this;
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	obstacle.setWidth(0.4f);
	obstacle.setHeight(0.5f);
	obstacle.setDepth(0.6f);
	obstacle.setPosition(0.2f, 0.2f, 0.2f);
	planWithSimpleSetup();

	cam.setNearClip(0.001f);
	cam.setTarget(boundbox);
	cam.setDistance(boundbox.getSize().length() * 2.f);
}

４．ofApp::drawにobstacleの描画ルーチンを追加

//--------------------------------------------------------------
void ofApp::draw(){
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundbox.drawWireframe();
	ofSetColor(ofColor::darkBlue, 66);
	obstacle.draw();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i < stateAxis.size(); i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundbox.getSize().length() / 10.f);
			ofPopMatrix();
		}
	}
	cam.end();
}

実行結果はこんな感じ。障害物を避けてパスをつないでいますね。
f:id:cvl-robot:20180510145231p:plain

[1] Robotics/OMPL - NAIST::OnlineText 幾何学的プラニングを参照

2018-05-06

openFrameworksでOMPLを動かしたい（その2）

ビルドの準備が整いましたので、ompl.slnを開いてビルドをしていきます。

ハマリポイント４
omplのEigenのパスが一部おかしい。

そのままビルドにかけると、Eigenのインクルードパスがおかしいとエラーが出ます。Linuxとかだと多分大丈夫なんでしょうけど、< eigen3と頭に余計なパスの指定があります。
ex.
#include < eigen3/Eigen/Core > -> #include
これを強引に全置換でeigen3を消してしまうことにします。一部のファイルでは、eigen3の無いパスを指定していますので、Eigen3を置く場所を本当にeigen3ディレクトリの下においてもあまり意味はありません。いっぱいあってうんざりしますが、全部置き換えると無事にビルドが通るようになります。

リンク用の空のofxOMPL.h

以前、別のライブラリで自前でリンクするならaddonはほぼ空でも良いんじゃね？という悪い方法を学んでしまいましたので、今回もその手で行きます。

ofxOMPL/src/ofxOMPL.h

#pragma once

// Plaease add these settings manually
// #include path example:               D:\workspace\ompl\src
//                                                     D:\workspace\eigen-334
//                                                     D:\workspace\boost_1_58_0
// #library path example:	                D:\workspace\ompl\build\release\lib\Release
//                                                     (D:\workspace\ompl\build\release\lib\Debug)
//                                                     D:\workspace\boost_1_58_0\stage\x64\lib

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>

#if _DEBUG
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#else
#pragma comment(lib, "ompl.lib")
#pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib")
#endif

OMPLの最初のtutorialを一部可視化しただけのofxOMPLRigidBodyPlanning

すみません、初歩的なミスを。CallBack関数にClassのメンバー関数であるofApp::isStateValid関数を直接設定することができません。修正しました。

main.cpp 変更なし

#include "ofMain.h"
#include "ofApp.h"

//========================================================================
int main( ){
	ofSetupOpenGL(1024,768,OF_WINDOW);			// <-------- setup the GL context

	// this kicks off the running of my app
	// can be OF_WINDOW or OF_FULLSCREEN
	// pass in width and height too:
	ofRunApp(new ofApp());

}

ofApp.h

#pragma once

#include "ofxOMPL.h"
#include "ofMain.h"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
		void planWithSimpleSetup();
		bool isStateValid(const ompl::base::State *state);

		std::shared_ptr<ompl::base::RealVectorBounds> bounds;
		std::shared_ptr<ompl::geometric::SimpleSetup> ss;
		ompl::base::PlannerStatus solved;
protected:
		ofEasyCam cam;
		ofBoxPrimitive boundbox;
		ofPolyline polyline;
		std::vector<ofMatrix4x4> stateAxis;
};

ofApp.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ofApp.h"

#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

ofApp *app;

bool isStateValid(const ob::State *state)
{
	return app->isStateValid(state);
}

bool ofApp::isStateValid(const ob::State *state)
{
	// cast the abstract state type to the type we expect
	const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

	// extract the first component of the state and cast it to what we expect
	const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

	// extract the second component of the state and cast it to what we expect
	const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

	// check validity of state defined by pos & rot


	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	return (const void*)rot != (const void*)pos;
}

void ofApp::planWithSimpleSetup()
{
	// construct the state space we are planning in
	auto space(std::make_shared<ob::SE3StateSpace>());

	// set the bounds for the R^3 part of SE(3)
	bounds = std::make_shared<ob::RealVectorBounds>(3);

	bounds->setLow(-1);
	bounds->setHigh(1);

	space->setBounds(*bounds);

	// define a simple setup class
	ss = std::make_shared<og::SimpleSetup>(space);

	// set state validity checking for this space
	ss->setStateValidityChecker([](const ob::State *state) { return ::isStateValid(state); });

	// create a random start state
	ob::ScopedState<> start(space);
	start.random();

	// create a random goal state
	ob::ScopedState<> goal(space);
	goal.random();

	// set the start and goal states
	ss->setStartAndGoalStates(start, goal);

	// this call is optional, but we put it in to get more output information
	ss->setup();
	ss->print();

	// attempt to solve the problem within one second of planning time
	solved = ss->solve(1.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen
		ss->simplifySolution();
		ss->getSolutionPath().print(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;

	// setups for drawings
	// bound
	std::vector<double> h = bounds->high;
	std::vector<double> l = bounds->low;
	ofVec3f high, low, vol;
	for (int i = 0; i < h.size(); i++) {
		high[i] = h[i];
		low[i] = l[i];
	}
	vol = high - low;
	boundbox.set(vol[0], vol[1], vol[2], 1, 1, 1);
	boundbox.setPosition((high + low) / 2.f);
	// SolutionPath
	if (solved)
	{
		stateAxis.clear();
		polyline.clear();
		for (int i = 0; i < ss->getSolutionPath().getStateCount(); i++) {
			std::vector<double> reals;
			ss->getStateSpace()->copyToReals(reals, ss->getSolutionPath().getState(i));

			ofMatrix4x4 mat;
			mat.setTranslation(reals[0], reals[1], reals[2]);
			mat.setRotate(ofQuaternion(reals[3], reals[4], reals[5], reals[6]));
			stateAxis.push_back(mat);
			polyline.addVertex(mat.getTranslation());
		}
	}
}

//--------------------------------------------------------------
void ofApp::setup(){
	app = this;
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	planWithSimpleSetup();

	cam.setNearClip(0.001f);
	cam.setTarget(boundbox);
	cam.setDistance(boundbox.getSize().length() * 2.f);
}

//--------------------------------------------------------------
void ofApp::update(){

}

//--------------------------------------------------------------
void ofApp::draw(){
	ofSetBackgroundColor(ofColor::black);

	cam.begin();
	ofPushStyle();
	ofSetColor(ofColor::blue);
	ofDrawGridPlane(0.01f);
	ofSetColor(ofColor::grey, 66);
	boundbox.drawWireframe();
	ofPopStyle();
	if (solved)
	{
		ofPushStyle();
		ofSetColor(ofColor::yellow);
		polyline.draw();
		ofPopStyle();
		for (unsigned int i = 0; i < stateAxis.size(); i++) {
			ofPushMatrix();
			ofMultMatrix(stateAxis[i]);
			ofDrawAxis(boundbox.getSize().length() / 10.f);
			ofPopMatrix();
		}
	}
	cam.end();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){

}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

実行結果はこんな感じ｡もともと何もしてないサンプルなので、今のところまだ何もしてくれません。
f:id:cvl-robot:20180508110607p:plain

2018-05-05

openFrameworksでOMPLを動かしたい（その1）

ロボットの経路計画や動作計画用ライブラリとして有名なOMPL（OpenMotionPlanningLibrary）をWindows10MSVC環境のopenFrameworksで動かせるようにしたいと思います。
The Open Motion Planning Library
当初は、C＋＋できれいに書かれているし単に.libをリンクするだけでいいんじゃない？と思っていましたし、結果的にはそうだったのですが、いろいろはまりポイントがあったので記録に残します。

インストールガイドによると、Windowsにインストールはできるけれどもサポート外でAppVeyorを参考にしてくれ、とあります。
Installation

ハマリポイント１

多分やり方が悪いのだと思うのですが、AppVeyorでビルドしたompl.libはうまくリンクできない。

appveyor版ビルドは使いませんが、ビルドの仕方だけ箇条書きで残します。またＧＵＩのompl.appはこの記事では扱いません。
１．appVeyorにgithubのアカウント連携を使ってアカウントを作る
２．OMPLを自分のGithubにフォークする
３．appVeyorにてProject->NewProject->Omplのフォークを指定
４．ビルドを開始。３０分ほどで完了、メールで教えてくれる
５．appVeyorにてompl->Artifacts->build/ompl-1.3.2-win64.zipをダウンロード
これを展開して生成物を普通にリンク指定して動かしてみようとしても、リンクの不具合が出てうまく解消できませんでした。

あきらめて、プロジェクトフォルダを眺めるとCMakeList.txtがあり、中を見るとMSVCの記述もあるので普通にCMakeでも良さそうです。
CMakeでビルドしようとみていくと、まずboostで引っかかることに気が付きました。

ハマリポイント２

boostが必要だけど、openFrameworksのboostじゃいろいろ足らない

omplは外部ライブラリとして、EigenとBoost(大きなライブラリにしては、だけ)を必要とします。
openFrameworksは、自身にboost1.58を内包しているのですが、全てではなく一部だけしか持っていません。なので、不足分を自前で用意する必要があります。

もっと新しいバージョンのboostでも多分大丈夫ですが、ここではopenFrameworks0.9.8に合わせてboost1.58を自前で用意したいと思います。
１．https://sourceforge.net/projects/boost/files/boost/からboost1.58.0.zipのソースコードをダウンロードする
２．VisualStudioの「開発者コマンドプロンプトforVS2015」を開き、bootstrap.batを実行します。
３．新しくできたb2.exeを使ってlibを作成[１].ちゃんとカレントディレクトリをbootstrap.batと同じ位置に移動してからじゃないと駄目ですよ。

b2 toolset=msvc threading=multi variant=debug,release link=static runtime-link=static address-model=32 --stagedir=stage/x86 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=shared runtime-link=shared address-model=32 --stagedir=stage/x86 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=static runtime-link=static address-model=64 --stagedir=stage/x64 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=shared runtime-link=shared address-model=64 --stagedir=stage/x64 -j 8

ハマリポイント３

omplのCMakeオプションの見えるところにBoostの表示がない

結果BOOST_ROOTでパスを指定すればよかっただけなのですが、見えるところになかったので焦りました。
CMakeの指定の例：
Where is the source code: D:\workspace\ompl
Where to build the binaries: D:\workspace\ompl\build\release
EIGEN3_INCLUDE_DIR: D:/workspace/eigen-334
Add Entry
Name: BOOST_ROOT
Type: PATH
Value: D:\workspace\boost_1_58_0
Add Entry
Name: BOOST_LIBRARYDIR
Type: PATH
Value: D:/workspace/boost_1_58_0/stage/x64/lib

ようやくompl.libが出来上がりました。つづく。

[１]或るプログラマの一生 » Windows に boost C++ libraries をインストール（ビルド）

cvl-robot's diary

研究ノート　メモメモ https://github.com/dotchang/

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法 (その2)任意のメッシュ形状でも衝突判定したい

ofxBullletCustomShapeクラスの改造

呼び出し方

今日の本文

動力学シミュレータofxBulletを静的な衝突判定ライブラリとしてだけ使う方法

衝突判定ライブラリ探し

衝突判定のサンプル

今日のマンガ

openFrameworksでOMPLを動かしたい（その5）

openFrameworksでOMPLを動かしたい（その4）

ofxOMPL.hの更新

myRigidBodyPlanningWithControlsプロジェクト

openFrameworksでOMPLを動かしたい（その3）

openFrameworksでOMPLを動かしたい（その2）

リンク用の空のofxOMPL.h

OMPLの最初のtutorialを一部可視化しただけのofxOMPLRigidBodyPlanning

openFrameworksでOMPLを動かしたい（その1）

ハマリポイント１

ハマリポイント２

ハマリポイント３