Currently job artifacts in CI/CD pipelines on LRZ GitLab never expire. Starting from Wed 26.1.2022 the default expiration time will be 30 days (GitLab default). Currently existing artifacts in already completed jobs will not be affected by the change. The latest artifacts for all jobs in the latest successful pipelines will be kept. More information: https://gitlab.lrz.de/help/user/admin_area/settings/continuous_integration.html#default-artifacts-expiration

Commit e5a02a1f authored by Mahmoud Mahmoud's avatar Mahmoud Mahmoud
Browse files

khepera example

parent 7c53e80c
# #
# compiler # compiler
# #
#CC = g++ CC = g++
CC = clang++ #CC = clang++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
# #
......
...@@ -31,7 +31,7 @@ typedef std::array<double,2> state_type; ...@@ -31,7 +31,7 @@ typedef std::array<double,2> state_type;
typedef std::array<double,1> input_type; typedef std::array<double,1> input_type;
/* sampling time */ /* sampling time */
const double tau = 0.5; const double tau = 0.05;
/* number of intermediate steps in the ode solver */ /* number of intermediate steps in the ode solver */
const int nint=5; const int nint=5;
OdeSolver ode_solver(sDIM,nint,tau); OdeSolver ode_solver(sDIM,nint,tau);
......
# #
# compiler # compiler
# #
#CC = g++ CC = g++
CC = clang++
CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG CXXFLAGS = -Wall -Wextra -std=c++11 -O3 -DNDEBUG
CXXFLAGS = -Wall -Wextra -std=c++11 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
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