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)です。回転の単位はラジアン。制御のパラメータは、(前進速度、回転速度、期間)です。
よくわからないまま動かしてみると、こんな結果になります。

なんかよくわからないふらふらした動きしてますね。