cvl-robot's diary

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

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) {

}

実行結果はこんな感じ。障害物を避けてスタートとゴールを結んでいます。
アニメーションは、よく見ると時々はみ出している・・・何かを間違えているっぽい気がする。。。
f:id:cvl-robot:20180510230334g:plain

[1] Robotics/OMPL - NAIST::OnlineText 制御指令値を含むプラニング