Unicycle example added

examples/hscc16/dcdc/readme

@@ -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 

examples/hscc16/unicycle/Makefile

+#
+# 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

examples/hscc16/unicycle/readme

+
+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
+
+

examples/hscc16/unicycle/unicycle.cc

+/*
+ * 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]);
+  }
+}
+
+

examples/hscc16/unicycle/unicycle.m

+%
+% 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

examples/hscc16/vehicle1/readme

@@ -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

examples/hscc16/vehicle2/readme

@@ -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

mfiles/SymbolicSet.m

@@ -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 ?']);

mfiles/mexfiles/mexSymbolicSetReadFromFile.cc

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

src/SymbolicModel.hh

@@ -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] << " ";
+    }