cvl-robot's diary

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

openFrameworksでOMPLを動かしたい(その3)

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

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

ofBoxPrimitive obstacle;

2.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;
}

3.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);
}

4.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 幾何学的プラニングを参照