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(); }
実行結果はこんな感じ。障害物を避けてパスをつないでいますね。
[1] Robotics/OMPL - NAIST::OnlineText 幾何学的プラニングを参照