openFrameworksでOMPLを動かしたい(その5)
また[1]を参考に、修正を加えてみます。
1.障害物を追加
2.cboundsの値を調整
3.アニメーションを追加
#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) { }
実行結果はこんな感じ。障害物を避けてスタートとゴールを結んでいます。
アニメーションは、よく見ると時々はみ出している・・・何かを間違えているっぽい気がする。。。
[1] Robotics/OMPL - NAIST::OnlineText 制御指令値を含むプラニング