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

Commit e5a02a1f authored by Mahmoud Mahmoud's avatar Mahmoud Mahmoud

khepera example

parent 7c53e80c
#
# compiler
#
#CC = g++
CC = clang++
CC = g++
#CC = clang++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
#
......
......@@ -31,7 +31,7 @@ typedef std::array<double,2> state_type;
typedef std::array<double,1> input_type;
/* sampling time */
const double tau = 0.5;
const double tau = 0.05;
/* number of intermediate steps in the ode solver */
const int nint=5;
OdeSolver ode_solver(sDIM,nint,tau);
......
#
# compiler
#
#CC = g++
CC = clang++
CC = g++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
CXXFLAGS = -Wall -Wextra -std=c++11
......
#
# compiler
#
CC = g++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
CXXFLAGS = -Wall -Wextra -std=c++11
#
# scots
#
SCOTSROOT = ../..
SCOTSINC = -I$(SCOTSROOT)/utils -I$(SCOTSROOT)/bdd
#
# cudd
#
CUDDPATH = /opt/local/
CUDDINC = -I$(CUDDPATH)/include
CUDDLIBS = -lcudd
CUDDLPATH = -L$(CUDDPATH)/lib
TARGET = khepera
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 ./*.bdd
/*
* unicycle.cc
*
* created on: 21.03.2018
* author: m. khaled
*/
#include <array>
#include <iostream>
#include "cuddObj.hh"
#include "SymbolicSet.hh"
#include "SymbolicModelGrowthBound.hh"
#include "TicToc.hh"
#include "RungeKutta4.hh"
#include "FixedPoint.hh"
/* state space dim */
#define sDIM 3
#define iDIM 2
/* data types for the ode solver */
typedef std::array<double,sDIM> state_type;
typedef std::array<double,iDIM> input_type;
/* sampling time */
const double tau = 0.3;
/* number of intermediate steps in the ode solver */
const int nint=5;
OdeSolver ode_solver(sDIM,nint,tau);
/* we integrate the unicycle ode by 0.3 sec (the result is stored in x) */
auto khepera_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];
};
ode_solver(rhs,x,u);
};
/* 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]*std::abs(u[0])*0.3;
r[1] = r[1]+r[2]*std::abs(u[0])*0.3;
};
scots::SymbolicSet kheperaCreateStateSpace(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]={15,13,M_PI+0.4};
/* grid node distance diameter */
double eta[sDIM]={.1,.1,.1};
scots::SymbolicSet ss(mgr,sDIM,lb,ub,eta);
/* add the grid points to the SymbolicSet ss */
ss.addGridPoints();
double H[4*sDIM]={-1, 0, 0,
1, 0, 0,
0,-1, 0,
0, 1, 0};
#define num_obstacles 34
double h[num_obstacles][4] = {
{-0.0, 0.5, - 8.2, 13},
{-0.5, 1.5, -10.5, 13},
{-1.5, 2.2, - 8.2, 13},
{-1.5, 2.2, -6.3, 7.0},
{-0.0, 0.5, -1.2, 5},
{-0.5, 1.5, -1.2, 3},
{-1.5, 2.2, -1.2, 5},
/////////////////////////////////////////
{-3.0, 3.3, - 0.0, 1.5},
{-3.0, 3.3, - 3.7, 5.2},
{-3.0, 3.3, - 7.5, 8.9},
{-3.0, 3.3, -10.8, 12.2},
/////////////////////////////////////////
{-4.2, 6.8, -10.8, 11.3},
{-6.2, 6.8, -11.3, 13},
{-4.2, 6.8, - 9.6, 10.0},
{-4.2, 6.8, - 7.9, 8.5},
{-4.2, 4.8, - 4.9, 7.9},
{-4.2, 6.8, - 3.5, 6.0},
{-5.1, 6.8, - 2.4, 2.8},
{-4.2, 6.8, - 0.0, 1.5},
/////////////////////////////////////////
{-7.7, 8.0, - 11 , 13},
{-7.7, 8.0, - 3.8, 8.1},
{-7.7, 8.0, - 0.0, 1.3},
/////////////////////////////////////////
{-9.1, 11.2, -10.8, 13},
{-9.1, 11.2, - 9.6, 10.0},
{-9.1, 11.2, - 6.8, 8.5},
{-9.1, 11.2, - 3.8, 4.4},
{-9.1, 10.2, - 2.4, 2.8},
{-8.9, 15.0, - 0.0, 1.5},
/////////////////////////////////////////
{-12.0, 12.3, - 11 , 13},
{-12.0, 12.3, - 6.2, 8.1},
{-12.0, 12.3, - 3.8, 4.9},
/////////////////////////////////////////
{-13.2, 15, - 6.8, 11.5},
{-13.2, 14, - 5.6, 5.9},
{-13.2, 15, - 3.6, 4.7},
};
for(size_t i=0; i<num_obstacles; i++){
ss.remPolytope(4,H,h[i], scots::OUTER);
}
return ss;
}
scots::SymbolicSet kheperaCreateInputSpace(Cudd &mgr) {
/* lower bounds of the hyper rectangle */
double lb[sDIM]={-1,-1.5};
/* upper bounds of the hyper rectangle */
double ub[sDIM]={1 , 1.5};
/* grid node distance diameter */
double eta[sDIM]={.3,.2};
scots::SymbolicSet is(mgr,iDIM,lb,ub,eta);
is.addGridPoints();
return is;
}
scots::SymbolicSet kheperaCreateTargetSet(scots::SymbolicSet ss) {
scots::SymbolicSet ts = ss;
ts.clear();
double H[4*sDIM]={-1, 0, 0,
1, 0, 0,
0,-1, 0,
0, 1, 0};
double h[4] = {-3.0, 3.3, - 2.2, 2.6};
ts.addPolytope(4, H,h, scots::OUTER);
return ts;
}
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=kheperaCreateStateSpace(mgr);
ss.writeToFile("khepera_ss.bdd");
ss.complement();
ss.writeToFile("khepera_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 = kheperaCreateTargetSet(ss);
ts.writeToFile("khepera_target.bdd");
/****************************************************************************/
/* construct SymbolicSet for the input space */
/****************************************************************************/
scots::SymbolicSet is=kheperaCreateInputSpace(mgr);
std::cout << std::endl << "Input space details:" << std::endl;
is.printInfo(1);
/****************************************************************************/
/* setup class for symbolic model computation */
/****************************************************************************/
/* first create SymbolicSet of post variables
* by copying the SymbolicSet of the state space and assigning new BDD IDs */
scots::SymbolicSet sspost(ss,1);
/* instantiate the SymbolicModel */
scots::SymbolicModelGrowthBound<state_type,input_type> abstraction(&ss, &is, &sspost);
/* compute the transition relation */
tt.tic();
abstraction.computeTransitionRelation(khepera_post, khepera_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;
/****************************************************************************/
/* we continue with the controller synthesis */
/****************************************************************************/
int verbose=1;
/* we setup a fixed point object to compute reachabilty controller */
scots::FixedPoint fp(&abstraction);
/* the fixed point algorithm operates on the BDD directly */
BDD T = ts.getSymbolicSet();
tt.tic();
BDD C = fp.reach(T,verbose);
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(C);
controller.writeToFile("khepera_controller.bdd");
scots::SymbolicSet tr = abstraction.getTransitionRelation();
tr.writeToFile("khepera_transition_relation.bdd");
return 1;
}
%
% khepera.m
%
% originally on: 21.01.2016 by [rungger]
% author: M. Khaled
%
% you need to run ./khepera binary first
%
% so that the files: khepera_ss.bdd
% khepera_obst.bdd
% khepera_target.bdd
% khepera_controller.bdd
% are created
%
function unicycle
clear set
close all
%% init
L=[2 0 0; 0 1 0; 0 0 .1];
c=[9.5; 0.6; 0];
% colors
colors=get(groot,'DefaultAxesColorOrder');
% initial state
x0=[12.5 10 -pi/2];
% load the symbolic set containig the abstract state space
% set=SymbolicSet('khepera_ss.bdd','projection',[1 2]);
% plotCells(set,'facecolor','none','edgec',[0.8 0.8 0.8],'linew',.1)
hold on
box on
axis([-.5 15.5 -.5 13.5])
% load the symbolic set containig obstacles
set=SymbolicSet('khepera_obst.bdd','projection',[1 2]);
plotCells(set,'facecolor',colors(1,:)*0.5+0.5,'edgec',colors(1,:),'linew',.1)
% load the symbolic set containig target set
set=SymbolicSet('khepera_target.bdd','projection',[1 2]);
plotCells(set,'facecolor',colors(2,:)*0.5+0.5,'edgec',colors(2,:),'linew',.1)
%% simulation
controller=SymbolicSet('khepera_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(@khepera_ode,[0 .3], y(end,:),[],u(1,:));
y=[y; x(end,:)];
end
% plot initial state and trajectory
plot(y(:,1),y(:,2),'k.-')
plot(y(1,1),y(1,2),'.','color',colors(5,:),'markersize',20)
end
function dxdt = khepera_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
0 - 130 * 0.2
0 - 130 * 0.1
\ No newline at end of file
#
# matlab root
#
MATLABROOT = /Applications/MATLAB_R2016b.app/
MATLABROOT = ~/installed/MATLAB/R2017b/
#
# compiler
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment