2.12.2021, 9:00 - 11:00: Due to updates GitLab may be unavailable for some minutes between 09:00 and 11:00.

Commit e95ccc88 authored by Matthias Rungger's avatar Matthias Rungger
Browse files

Unicycle example added

parent e78181b8
......@@ -25,7 +25,7 @@ specification
b) in Matlab add the path and run the simulation
>> addpath(genpath('$(SCOTSROOT)/mfiles'))
>> addpath(genpath('../../../mfiles'))
>> dcdc
4. if you want to compare with Pessoa
......
#
# compiler
#
#CC = g++
CC = clang++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
CXXFLAGS = -Wall -Wextra -std=c++11
#
# scots
#
SCOTSROOT = ../../..
SCOTSINC = -I$(SCOTSROOT)/src
#
# cudd
#
CUDDPATH = /opt/local/
CUDDINC = -I$(CUDDPATH)/include
CUDDLIBS = -lcudd
CUDDLPATH = -L$(CUDDPATH)/lib
TARGET = unicycle
all: $(TARGET)
%.o:%.cc
$(CC) -c $(CXXFLAGS) $(CUDDINC) $(SCOTSINC) $< -o $@
$(TARGET): $(TARGET).o
$(CC) $(CXXFLAGS) -o $(TARGET) $(TARGET).o $(CUDDLPATH) $(CUDDLIBS)
clean:
rm ./$(TARGET) ./$(TARGET).o
A path planning problem for a unicycle
1. compile the unicycle.cc file
a) edit the Makefile and adjust the CUDDPATH to the location where the cudd library is installed.
b) you should be able to compile the program simply by
$ make
2. execute
$./unicycle
which produces
./unicycle_ss.bdd
./unicycle_obst.bdd
./unicycle_target.bdd
./unicycle_controller.bdd
3. simulate the closed loop in Matlab
a) you need to compile the mexfile first (see the readme in $(SCOTSROOT)/mfiles/mexfiles )
b) in Matlab add the path and run the simulation
>> addpath(genpath('../../../mfiles'))
>> unicycle
/*
* unicycle.cc
*
* created on: 21.01.2016
* author: rungger
*/
/*
* information about this example is given in the readme file
*
*/
#include <array>
#include <iostream>
#include "cuddObj.hh"
#include "SymbolicSet.hh"
#include "SymbolicModelGrowthBound.hh"
#include "TicToc.hh"
#include "FixedPoint.hh"
/* state space dim */
#define sDIM 3
#define iDIM 2
/* data types for the ode solver */
typedef std::array<double,3> state_type;
typedef std::array<double,2> input_type;
/* forward declaration of the ode solver */
template<class F>
void ode_solver(F rhs, state_type &x, input_type &u, size_t nint, double h);
/* we integrate the unicycle ode by 0.3 sec (the result is stored in x) */
auto unicycle_post = [](state_type &x, input_type &u) -> void {
/* the ode describing the unicycle */
auto rhs =[](state_type& xx, const state_type &x, input_type &u) -> void {
xx[0] = u[0]*std::cos(x[2]);
xx[1] = u[0]*std::sin(x[2]);
xx[2] = u[1];
};
size_t nint=5; /* number of intermediate step size */
double h=0.06; /* h* nint = sampling time */
ode_solver(rhs,x,u,nint,h);
};
/* computation of the growth bound (the result is stored in r) */
auto radius_post = [](state_type &r, input_type &u) -> void {
r[0] = r[0]+r[2]*u[0]*0.3;
r[1] = r[1]+r[2]*u[0]*0.3;
};
/* forward declaration of the functions to setup the state space
* and input space of the unicycle example */
scots::SymbolicSet unicycleCreateStateSpace(Cudd &mgr);
scots::SymbolicSet unicycleCreateInputSpace(Cudd &mgr);
int main() {
/* to measure time */
TicToc tt;
/* there is one unique manager to organize the bdd variables */
Cudd mgr;
/****************************************************************************/
/* construct SymbolicSet for the state space */
/****************************************************************************/
scots::SymbolicSet ss=unicycleCreateStateSpace(mgr);
ss.writeToFile("unicycle_ss.bdd");
/* write SymbolicSet of obstacles to unicycle_obst.bdd */
ss.complement();
ss.writeToFile("unicycle_obst.bdd");
ss.complement();
std::cout << "Unfiorm grid details:" << std::endl;
ss.printInfo(1);
/****************************************************************************/
/* the target set */
/****************************************************************************/
/* first make a copy of the state space so that we obtain the grid
* information in the new symbolic set */
scots::SymbolicSet ts = ss;
/* define the target set as a symbolic set */
double H[9]={ 2, 0, 0,
0, 1, 0,
0, 0, .1};
/* compute inner approximation of P={ x | H x<= h1 } */
double c[3] = {9.5,0.6,0};
ts.addEllipsoid(H,c, scots::INNER);
ts.writeToFile("unicycle_target.bdd");
/****************************************************************************/
/* construct SymbolicSet for the input space */
/****************************************************************************/
scots::SymbolicSet is=unicycleCreateInputSpace(mgr);
std::cout << std::endl << "Input space details:" << std::endl;
is.printInfo(1);
/****************************************************************************/
/* setup class for symbolic model computation */
/****************************************************************************/
scots::SymbolicModelGrowthBound<state_type,input_type> abstraction(&ss, &is);
/* compute the transition relation */
tt.tic();
abstraction.computeTransitionRelation(unicycle_post, radius_post);
std::cout << std::endl;
tt.toc();
/* get the number of elements in the transition relation */
std::cout << std::endl << "Number of elements in the transition relation: " << abstraction.getSize() << std::endl;
abstraction.writeToFile("unicycle_abstraction.bdd");
/****************************************************************************/
/* we continue with the controller synthesis */
/****************************************************************************/
/* we setup a fixed point object to compute reachabilty controller */
scots::FixedPoint reach(&abstraction);
/* the fixed point algorithm operates on the BDD directly */
BDD bddTarget = ts.getSymbolicSet();
/* the output of the fixed point computation are two bdd's */
BDD bddFixedPoint; /* contains the state space grid points of the fixed point*/
BDD bddController; /* contains the state space grid points and associated inputs */
tt.tic();
reach.minimalFixedPoint(bddTarget, bddFixedPoint, bddController, 1);
tt.toc();
/****************************************************************************/
/* last we store the controller as a SymbolicSet
* the underlying uniform grid is given by the Cartesian product of
* the uniform gird of the space and uniform gird of the input space */
/****************************************************************************/
scots::SymbolicSet controller(ss,is);
controller.setSymbolicSet(bddController);
controller.writeToFile("unicycle_controller.bdd");
return 1;
}
scots::SymbolicSet unicycleCreateStateSpace(Cudd &mgr) {
/* setup the workspace of the synthesis problem and the uniform grid */
/* lower bounds of the hyper rectangle */
double lb[sDIM]={0,0,-M_PI-0.4};
/* upper bounds of the hyper rectangle */
double ub[sDIM]={10,10,M_PI+0.4};
/* grid node distance diameter */
double eta[sDIM]={.2,.2,.2};
/* eta is added to the bound so as to ensure that the whole
* [0,10]x[0,10]x[-pi-eta,pi+eta] is covered by the cells */
scots::SymbolicSet ss(mgr,sDIM,lb,ub,eta);
/* add the grid points to the SymbolicSet ss */
ss.addGridPoints();
/* remove the obstacles from the state space */
/* the obstacles are defined as polytopes */
/* define H* x <= h */
double H[4*sDIM]={-1, 0, 0,
1, 0, 0,
0,-1, 0,
0, 1, 0};
/* remove outer approximation of P={ x | H x<= h1 } form state space */
double h1[4] = {-1,1.2,-0, 9};
ss.remPolytope(4,H,h1, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h2 } form state space */
double h2[4] = {-2.2,2.4,-0,5};
ss.remPolytope(4,H,h2, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h3 } form state space */
double h3[4] = {-2.2,2.4,-6,10};
ss.remPolytope(4,H,h3, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h4 } form state space */
double h4[4] = {-3.4,3.6,-0,9};
ss.remPolytope(4,H,h4, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h5 } form state space */
double h5[4] = {-4.6 ,4.8,-1,10};
ss.remPolytope(4,H,h5, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h6 } form state space */
double h6[4] = {-5.8,6,-0,6};
ss.remPolytope(4,H,h6, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h7 } form state space */
double h7[4] = {-5.8,6,-7,10};
ss.remPolytope(4,H,h7, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h8 } form state space */
double h8[4] = {-7,7.2,-1,10};
ss.remPolytope(4,H,h8, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h9 } form state space */
double h9[4] = {-8.2,8.4,-0,8.5};
ss.remPolytope(4,H,h9, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h10 } form state space */
double h10[4] = {-8.4,9.3,-8.3,8.5};
ss.remPolytope(4,H,h10, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h11 } form state space */
double h11[4] = {-9.3,10,-7.1,7.3};
ss.remPolytope(4,H,h11, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h12 } form state space */
double h12[4] = {-8.4,9.3,-5.9,6.1};
ss.remPolytope(4,H,h12, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h13 } form state space */
double h13[4] = {-9.3,10 ,-4.7,4.9};
ss.remPolytope(4,H,h13, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h14 } form state space */
double h14[4] = {-8.4,9.3,-3.5,3.7};
ss.remPolytope(4,H,h14, scots::OUTER);
/* remove outer approximation of P={ x | H x<= h15 } form state space */
double h15[4] = {-9.3,10 ,-2.3,2.5};
ss.remPolytope(4,H,h15, scots::OUTER);
return ss;
}
scots::SymbolicSet unicycleCreateInputSpace(Cudd &mgr) {
/* lower bounds of the hyper rectangle */
double lb[sDIM]={-1,-1};
/* upper bounds of the hyper rectangle */
double ub[sDIM]={1,1};
/* grid node distance diameter */
double eta[sDIM]={.3,.3};
scots::SymbolicSet is(mgr,iDIM,lb,ub,eta);
is.addGridPoints();
return is;
}
template<class F>
void ode_solver(F rhs, state_type &x, input_type &u, size_t nint, double h) {
/* runge kutte order 4 */
state_type k[4];
state_type tmp;
for(size_t t=0; t<nint; t++) {
rhs(k[0],x, u);
for(size_t i=0;i<sDIM;i++)
tmp[i]=x[i]+h/2*k[0][i];
rhs(k[1],tmp, u);
for(size_t i=0;i<sDIM;i++)
tmp[i]=x[i]+h/2*k[1][i];
rhs(k[2],tmp, u);
for(size_t i=0;i<sDIM;i++)
tmp[i]=x[i]+h*k[2][i];
rhs(k[3],tmp, u);
for(size_t i=0; i<sDIM; i++)
x[i] = x[i] + (h/6)*(k[0][i] + 2*k[1][i] + 2*k[2][i] + k[3][i]);
}
}
%
% unicycle.m
%
% created on: 21.01.2016
% author: rungger
%
% see readme file for more information on the unicycle example
%
% you need to run ./unicycle binary first
%
% so that the files: unicycle_ss.bdd
% unicycle_obst.bdd
% unicycle_target.bdd
% unicycle_controller.bdd
% are created
%
function unicycle
clear set
close all
%% simulation
%% target set
L=[2 0 0; 0 1 0; 0 0 .1];
c=[9.5; 0.6; 0];
% initial state
x0=[0.5 0.5 pi/2];
controller=SymbolicSet('unicycle_controller.bdd','projection',[1 2 3]);
y=x0;
v=[];
while(1)
if ( (y(end,:)-c')*L'*L*(y(end,:)'-c)<=1 )
break;
end
u=controller.getInputs(y(end,:));
v=[v; u(1,:)];
[t x]=ode45(@unicycle_ode,[0 .3], y(end,:),[],u(1,:));
y=[y; x(end,:)];
end
%% plot the unicycle domain
% colors
colors=get(groot,'DefaultAxesColorOrder');
% load the symbolic set containig the abstract state space
set=SymbolicSet('unicycle_ss.bdd','projection',[1 2]);
plotCells(set,'facecolor','none','edgec',[0.8 0.8 0.8],'linew',.1)
hold on
% load the symbolic set containig obstacles
set=SymbolicSet('unicycle_obst.bdd','projection',[1 2]);
plotCells(set,'facecolor',colors(1,:)*0.5+0.5,'edgec',colors(1,:),'linew',.1)
% plot the real obstacles and target set
plot_domain
% load the symbolic set containig target set
set=SymbolicSet('unicycle_target.bdd','projection',[1 2]);
plotCells(set,'facecolor',colors(2,:)*0.5+0.5,'edgec',colors(2,:),'linew',.1)
% plot initial state and trajectory
plot(y(:,1),y(:,2),'k.-')
plot(y(1,1),y(1,1),'.','color',colors(5,:),'markersize',20)
box on
axis([-.5 10.5 -.5 10.5])
end
function dxdt = unicycle_ode(t,x,u)
dxdt = zeros(3,1);
dxdt(1)=u(1)*cos(x(3));
dxdt(2)=u(1)*sin(x(3));
dxdt(3)=u(2);
end
function plot_domain
colors=get(groot,'DefaultAxesColorOrder');
v=[1 0 ;1.2 0 ; 1 9 ; 1.2 9 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[2.2 0 ;2.4 0 ; 2.2 5 ; 2.4 5 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[2.2 6 ;2.4 6 ; 2.2 10 ; 2.4 10 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[3.4 0 ;3.6 0 ; 3.4 9 ; 3.6 9 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[4.6 1 ;4.8 1 ; 4.6 10 ; 4.8 10 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[5.8 0 ;6 0 ; 5.8 6 ; 6 6 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[5.8 7 ;6 7 ; 5.8 10 ; 6 10 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[7 1 ;7.2 1 ; 7 10 ; 7.2 10 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[8.2 0 ;8.4 0 ; 8.2 8.5 ; 8.4 8.5 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[8.4 8.3;9.3 8.3 ; 8.4 8.5 ; 9.3 8.5 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[9.3 7.1;10 7.1 ; 9.3 7.3 ; 10 7.3 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[8.4 5.9;9.3 5.9 ; 8.4 6.1 ; 9.3 6.1 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[9.3 4.7;10 4.7 ; 9.3 4.9 ; 10 4.9 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[8.4 3.5;9.3 3.5 ; 8.4 3.7 ; 9.3 3.7 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
v=[9.3 2.3;10 2.3 ; 9.3 2.5 ; 10 2.5 ];
patch('vertices',v,'faces',[1 2 4 3],'facec',colors(1,:),'edgec',colors(1,:));
end
......@@ -26,7 +26,7 @@ A path planning problem for a vehicle using the bycicle model
b) in Matlab add the path and run the simulation
>> addpath(genpath('$(SCOTSROOT)/mfiles'))
>> addpath(genpath('../../../mfiles'))
>> vehicle
4. information on the example are found in
......
......@@ -26,7 +26,7 @@ A path planning problem for a vehicle using the bycicle model
b) in Matlab add the path and run the simulation
>> addpath(genpath('$(SCOTSROOT)/mfiles'))
>> addpath(genpath('../../../mfiles'))
>> vehicle
4. information on the example are found in
......
......@@ -94,6 +94,13 @@ classdef SymbolicSet < handle
error(['The state is not in the domain of the controller stored in:',obj.filename]);
end
end
function u=setValuedMap(obj,x)
x=x(:);
u=mexSymbolicSetReadFromFile(obj.filename,'setvaluedmap',x);
if(isempty(u))
error(['The state is not in the domain of the controller stored in:',obj.filename]);
end
end
function plot(obj,varargin)
if(obj.points==0)
error(['No grid points stored to be plotted. Did you run set.points ?']);
......
......@@ -83,14 +83,14 @@ void mexFunction(
mwSize n = mxGetN(prhs[2]);
double* ptr = mxGetPr(prhs[2]);
std::vector<size_t> projectdim;
for(size_t i=0; i<n; i++)
for(size_t i=0; i<n; i++)
projectdim.push_back((size_t)ptr[i]);
/* laod set */
Cudd mgr;
scots::SymbolicSet set(mgr,filename);
size_t dim=set.getDimension();
/* check sanity of project dimensions */
if(n>= dim)
if(n> dim)
mexErrMsgIdAndTxt( "MATLAB:mexSymbolicSetReadFromFile", "Number of project dimensions cannot exceed the number of dimensions.");
for(size_t i=0; i<n; i++) {
if(projectdim[i]>=dim)
......
......@@ -111,6 +111,11 @@ public:
inline double getSize(void) {
return transitionRelation_.CountMinterm(2*nssVars_+nisVars_);
};
/* function: getTransitionRelation
* get the BDD which represents the transition relation */
inline BDD getTransitionRelation(void) {
return transitionRelation_;
};
/* iterator methods */
/* function: begin
* initilize the iterator and compute the first element */
......@@ -152,6 +157,112 @@ public:
else
return NULL;
}
/* function: writeToFile
* stores the bdd of transition function as a <SymbolicSet> with all information to file*/
void writeToFile(const char *filename) {
/* produce string with parameters to be written into file */
std::stringstream dim;
std::stringstream z;
std::stringstream eta;
std::stringstream first;
std::stringstream last;
std::stringstream nofGridPoints;
std::stringstream indBddVars;
size_t dim_ = 2*stateSpace_->dim_+inputSpace_->dim_;
dim <<"#scots dimension: " << dim_ << std::endl;
z << "#scots measurement error bound: ";
eta << "#scots grid parameter eta: ";
first << "#scots coordinate of first grid point: ";
last << "#scots coordinate of last grid point: ";
nofGridPoints << "#scots number of grid points (per dim): ";
indBddVars << "#scots index of bdd variables: ";
/* write information of the pre state space */
for(size_t i=0; i<stateSpace_->dim_; i++) {
z << stateSpace_->z_[i] << " ";
eta << stateSpace_->eta_[i] << " ";
first << stateSpace_->firstGridPoint_[i] << " ";
last << stateSpace_->lastGridPoint_[i] << " ";
nofGridPoints << stateSpace_->nofGridPoints_[i] << " ";
}
for(size_t j=0; j<nssVars_; j++)
indBddVars << preVars_[j] << " ";
/* write information of the input space */
for(size_t i=0; i<inputSpace_->dim_; i++) {
z << inputSpace_->z_[i] << " ";
eta << inputSpace_->eta_[i] << " ";
first << inputSpace_->firstGridPoint_[i] << " ";
last << inputSpace_->lastGridPoint_[i] << " ";
nofGridPoints << inputSpace_->nofGridPoints_[i] << " ";
}
for(size_t j=0; j<nisVars_; j++)
indBddVars << inpVars_[j] << " ";
/* write information of the post state space */
for(size_t i=0; i<stateSpace_->dim_; i++) {
z << stateSpace_->z_[i] << " ";
eta << stateSpace_->eta_[i] << " ";
first << stateSpace_->firstGridPoint_[i] << " ";
last << stateSpace_->lastGridPoint_[i] << " ";
nofGridPoints << stateSpace_->nofGridPoints_[i] << " ";
}