cvl-robot's diary

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

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