├── include ├── smp │ ├── costtogo │ │ ├── costotogo_ref.rsp │ │ ├── codeInfo.mat │ │ ├── buildInfo.mat │ │ ├── html │ │ │ ├── inference.mat │ │ │ ├── coder_report.mat │ │ │ ├── hilite_warning.png │ │ │ ├── index.css │ │ │ ├── tansig_apply122_watch.html │ │ │ ├── tansig_apply95_watch.html │ │ │ ├── tansig_apply95_mcode.html │ │ │ ├── tansig_apply122_mcode.html │ │ │ ├── mapminmax_reverse157_mcode.html │ │ │ ├── mapminmax_reverse157_watch.html │ │ │ ├── mapminmax_apply47_watch.html │ │ │ ├── mapminmax_apply47_mcode.html │ │ │ ├── costotogo_buildlog1.html │ │ │ ├── costotogo_types_h.html │ │ │ ├── costotogo_terminate_c.html │ │ │ ├── costotogo_initialize_c.html │ │ │ └── contents_file.tmp │ │ ├── rtw_proj.tmw │ │ ├── costotogo_types.h │ │ ├── rtGetNaN.h │ │ ├── costotogo_initialize.c │ │ ├── costotogo_terminate.c │ │ ├── costotogo.h │ │ ├── rtGetInf.h │ │ ├── interface │ │ │ └── _coder_costotogo_info.h │ │ ├── costotogo_terminate.h │ │ ├── costotogo_initialize.h │ │ ├── rt_nonfinite.h │ │ ├── rt_nonfinite.c │ │ ├── rtGetNaN.c │ │ └── rtGetInf.c │ ├── components │ │ ├── multipurpose │ │ │ └── costtogo │ │ │ │ ├── costotogo_ref.rsp │ │ │ │ ├── buildInfo.mat │ │ │ │ ├── codeInfo.mat │ │ │ │ ├── html │ │ │ │ ├── inference.mat │ │ │ │ ├── coder_report.mat │ │ │ │ ├── hilite_warning.png │ │ │ │ ├── index.css │ │ │ │ ├── tansig_apply122_watch.html │ │ │ │ ├── tansig_apply95_watch.html │ │ │ │ ├── tansig_apply95_mcode.html │ │ │ │ ├── tansig_apply122_mcode.html │ │ │ │ ├── mapminmax_reverse157_mcode.html │ │ │ │ ├── mapminmax_reverse157_watch.html │ │ │ │ ├── mapminmax_apply47_watch.html │ │ │ │ ├── mapminmax_apply47_mcode.html │ │ │ │ ├── costotogo_buildlog1.html │ │ │ │ ├── costotogo_types_h.html │ │ │ │ ├── costotogo_terminate_c.html │ │ │ │ ├── costotogo_initialize_c.html │ │ │ │ └── contents_file.tmp │ │ │ │ ├── rtw_proj.tmw │ │ │ │ ├── costotogo_types.h │ │ │ │ ├── rtGetNaN.h │ │ │ │ ├── costotogo_initialize.c │ │ │ │ ├── costotogo_terminate.c │ │ │ │ ├── costotogo.h │ │ │ │ ├── rtGetInf.h │ │ │ │ ├── interface │ │ │ │ └── _coder_costotogo_info.h │ │ │ │ ├── costotogo_terminate.h │ │ │ │ ├── costotogo_initialize.h │ │ │ │ ├── rt_nonfinite.h │ │ │ │ ├── remove_svn.pl │ │ │ │ ├── rt_nonfinite.c │ │ │ │ ├── rtGetNaN.c │ │ │ │ └── rtGetInf.c │ │ ├── extenders │ │ │ ├── unicycleKinematics.m │ │ │ ├── base.hpp │ │ │ ├── input_array_double.hpp │ │ │ ├── state_array_double.hpp │ │ │ ├── input_array_double.h │ │ │ ├── state_array_double.h │ │ │ ├── MotionPrimitiveGeneratorCombiner.m │ │ │ ├── MotionPrimitiveGenerator.m │ │ │ ├── single_integrator.hpp │ │ │ └── dubins.h │ │ ├── samplers │ │ │ ├── base.hpp │ │ │ ├── gauss.h │ │ │ ├── halton.h │ │ │ ├── uniformc.h │ │ │ ├── halton.hpp │ │ │ ├── theta_star_gaussian.h │ │ │ ├── eigenmultivariatenormal.cpp │ │ │ ├── uniformc.hpp │ │ │ ├── uniform.h │ │ │ ├── gauss.hpp │ │ │ ├── opra.h │ │ │ ├── theta_star_gaussian.hpp │ │ │ └── uniform.hpp │ │ ├── distance_evaluators │ │ │ └── base.hpp │ │ ├── cost_evaluators │ │ │ ├── base.hpp │ │ │ ├── time.hpp │ │ │ ├── time.h │ │ │ └── base.h │ │ ├── model_checkers │ │ │ ├── base.hpp │ │ │ ├── reachability.hpp │ │ │ └── mu_calculus.h │ │ └── collision_checkers │ │ │ └── base.hpp │ ├── interfaces │ │ ├── base.hpp │ │ └── base.h │ ├── planner_utils │ │ ├── vertex_edge.hpp │ │ ├── trajectory.hpp │ │ └── trajectory.h │ ├── common │ │ ├── region.hpp │ │ ├── regionc.hpp │ │ ├── region.h │ │ └── regionc.h │ ├── utils │ │ ├── branch_and_bound_base.hpp │ │ └── branch_and_bound_euclidean.h │ ├── planners │ │ ├── base_incremental.hpp │ │ ├── planner_parameters.hpp │ │ └── rrt.hpp │ └── external_libraries │ │ ├── halton │ │ └── halton.h │ │ └── inc_mu_mc │ │ └── pt.h └── srl_global_planner │ ├── cell.cpp │ ├── cell.h │ ├── grid.h │ ├── trajectory.cpp │ └── srl_trajectory.h ├── config ├── move_base.yaml ├── common_params.yaml ├── dwa_local_planner_params.yaml ├── local_costmap_params.yaml ├── global_costmap_params_simulated.yaml └── global_costmap_params.yaml ├── srl_global_planner_plugin.xml ├── LICENSE └── package.xml /include/smp/costtogo/costotogo_ref.rsp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_ref.rsp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /config/move_base.yaml: -------------------------------------------------------------------------------- 1 | # control loop will run at 5 hz 2 | controller_frequency: 10.0 3 | -------------------------------------------------------------------------------- /include/smp/costtogo/codeInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/costtogo/codeInfo.mat -------------------------------------------------------------------------------- /include/smp/costtogo/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/costtogo/buildInfo.mat -------------------------------------------------------------------------------- /include/smp/costtogo/html/inference.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/costtogo/html/inference.mat -------------------------------------------------------------------------------- /include/smp/costtogo/html/coder_report.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/costtogo/html/coder_report.mat -------------------------------------------------------------------------------- /include/smp/costtogo/html/hilite_warning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/costtogo/html/hilite_warning.png -------------------------------------------------------------------------------- /include/smp/components/extenders/unicycleKinematics.m: -------------------------------------------------------------------------------- 1 | function dq = unicycleKinematics( t, q, v ,omega ) 2 | 3 | dq = [ v*cos(q(3)); v*sin(q(3)); omega ]; 4 | 5 | 6 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/buildInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/components/multipurpose/costtogo/buildInfo.mat -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/codeInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/components/multipurpose/costtogo/codeInfo.mat -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/inference.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/components/multipurpose/costtogo/html/inference.mat -------------------------------------------------------------------------------- /include/smp/costtogo/html/index.css: -------------------------------------------------------------------------------- 1 | body { 2 | font-family: sans-serif; 3 | font-size: 9pt; 4 | } 5 | pre.code, pre#RTWcode, .center .lineno { 6 | font:10pt/14pt Courier New; 7 | } 8 | -------------------------------------------------------------------------------- /include/smp/costtogo/rtw_proj.tmw: -------------------------------------------------------------------------------- 1 | Code generation project for costotogo using /usr/local/MATLAB/R2013b/toolbox/coder/coder/rtw/c/xrt/xrt_unix.tmf. MATLAB root = /usr/local/MATLAB/R2013b. 2 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/coder_report.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/components/multipurpose/costtogo/html/coder_report.mat -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/hilite_warning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srl-freiburg/srl_global_planner/HEAD/include/smp/components/multipurpose/costtogo/html/hilite_warning.png -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/index.css: -------------------------------------------------------------------------------- 1 | body { 2 | font-family: sans-serif; 3 | font-size: 9pt; 4 | } 5 | pre.code, pre#RTWcode, .center .lineno { 6 | font:10pt/14pt Courier New; 7 | } 8 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rtw_proj.tmw: -------------------------------------------------------------------------------- 1 | Code generation project for costotogo using /usr/local/MATLAB/R2013b/toolbox/coder/coder/rtw/c/xrt/xrt_unix.tmf. MATLAB root = /usr/local/MATLAB/R2013b. 2 | -------------------------------------------------------------------------------- /include/smp/interfaces/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_INTERFACE_BASE_HPP_ 2 | #define _SMP_INTERFACE_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /include/smp/components/samplers/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_BASE_HPP_ 2 | #define _SMP_SAMPLER_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /include/smp/components/distance_evaluators/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_DISTANCE_EVALUATOR_BASE_HPP_ 2 | #define _SMP_DISTANCE_EVALUATOR_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /include/srl_global_planner/cell.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | 5 | 6 | 7 | Cell::Cell(double x, double y, double w, double h) { 8 | hasAgent = false; 9 | occupied = false; 10 | } 11 | 12 | Cell::~Cell() { 13 | // clean up 14 | 15 | } 16 | -------------------------------------------------------------------------------- /include/smp/components/extenders/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_EXTEND_BASE_HPP_ 2 | #define _SMP_EXTEND_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /include/smp/components/cost_evaluators/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_COST_EVALUATOR_BASE_HPP_ 2 | #define _SMP_COST_EVALUATOR_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /include/smp/components/model_checkers/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_MODEL_CHECKER_BASE_HPP_ 2 | #define _SMP_MODEL_CHECKER_BASE_HPP_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /include/smp/components/collision_checkers/base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_COLLISION_CHECKER_BASE_HPP_ 2 | #define _SMP_COLLISION_CHECKER_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /srl_global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Global Planner consisting of Theta* (or A*) and RRT 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /include/srl_global_planner/cell.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | class Cell { 6 | // Constructor and Destructor 7 | public: 8 | //TODO: use QRectF? 9 | Cell(double x, double y, double w, double h); 10 | virtual ~Cell(); 11 | 12 | 13 | // Attributes 14 | public: 15 | 16 | 17 | bool hasAgent; 18 | bool occupied; 19 | double value; 20 | }; 21 | 22 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_types.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_TYPES_H__ 11 | #define __COSTOTOGO_TYPES_H__ 12 | 13 | /* Include files */ 14 | #include "rtwtypes.h" 15 | 16 | #endif 17 | /* End of code generation (costotogo_types.h) */ 18 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_types.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_TYPES_H__ 11 | #define __COSTOTOGO_TYPES_H__ 12 | 13 | /* Include files */ 14 | #include "rtwtypes.h" 15 | 16 | #endif 17 | /* End of code generation (costotogo_types.h) */ 18 | -------------------------------------------------------------------------------- /include/smp/costtogo/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetNaN.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RTGETNAN_H__ 11 | #define __RTGETNAN_H__ 12 | 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetNaN(void); 18 | extern real32_T rtGetNaNF(void); 19 | 20 | #endif 21 | /* End of code generation (rtGetNaN.h) */ 22 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetNaN.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RTGETNAN_H__ 11 | #define __RTGETNAN_H__ 12 | 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetNaN(void); 18 | extern real32_T rtGetNaNF(void); 19 | 20 | #endif 21 | /* End of code generation (rtGetNaN.h) */ 22 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo_initialize.c: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_initialize.c 3 | * 4 | * Code generation for function 'costotogo_initialize' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* Include files */ 11 | #include "rt_nonfinite.h" 12 | #include "costotogo.h" 13 | #include "costotogo_initialize.h" 14 | 15 | /* Function Definitions */ 16 | void costotogo_initialize(void) 17 | { 18 | rt_InitInfAndNaN(8U); 19 | } 20 | 21 | /* End of code generation (costotogo_initialize.c) */ 22 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo_terminate.c: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_terminate.c 3 | * 4 | * Code generation for function 'costotogo_terminate' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* Include files */ 11 | #include "rt_nonfinite.h" 12 | #include "costotogo.h" 13 | #include "costotogo_terminate.h" 14 | 15 | /* Function Definitions */ 16 | void costotogo_terminate(void) 17 | { 18 | /* (no terminate code required) */ 19 | } 20 | 21 | /* End of code generation (costotogo_terminate.c) */ 22 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_initialize.c: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_initialize.c 3 | * 4 | * Code generation for function 'costotogo_initialize' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* Include files */ 11 | #include "rt_nonfinite.h" 12 | #include "costotogo.h" 13 | #include "costotogo_initialize.h" 14 | 15 | /* Function Definitions */ 16 | void costotogo_initialize(void) 17 | { 18 | rt_InitInfAndNaN(8U); 19 | } 20 | 21 | /* End of code generation (costotogo_initialize.c) */ 22 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_terminate.c: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_terminate.c 3 | * 4 | * Code generation for function 'costotogo_terminate' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* Include files */ 11 | #include "rt_nonfinite.h" 12 | #include "costotogo.h" 13 | #include "costotogo_terminate.h" 14 | 15 | /* Function Definitions */ 16 | void costotogo_terminate(void) 17 | { 18 | /* (no terminate code required) */ 19 | } 20 | 21 | /* End of code generation (costotogo_terminate.c) */ 22 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_H__ 11 | #define __COSTOTOGO_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern double costotogo(const double x1[14]); 22 | #endif 23 | /* End of code generation (costotogo.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/costtogo/rtGetInf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetInf.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RTGETINF_H__ 11 | #define __RTGETINF_H__ 12 | 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetInf(void); 18 | extern real32_T rtGetInfF(void); 19 | extern real_T rtGetMinusInf(void); 20 | extern real32_T rtGetMinusInfF(void); 21 | 22 | #endif 23 | /* End of code generation (rtGetInf.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_H__ 11 | #define __COSTOTOGO_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern double costotogo(const double x1[14]); 22 | #endif 23 | /* End of code generation (costotogo.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rtGetInf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetInf.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RTGETINF_H__ 11 | #define __RTGETINF_H__ 12 | 13 | #include 14 | #include "rtwtypes.h" 15 | #include "rt_nonfinite.h" 16 | 17 | extern real_T rtGetInf(void); 18 | extern real32_T rtGetInfF(void); 19 | extern real_T rtGetMinusInf(void); 20 | extern real32_T rtGetMinusInfF(void); 21 | 22 | #endif 23 | /* End of code generation (rtGetInf.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/costtogo/interface/_coder_costotogo_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _coder_costotogo_info.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef ___CODER_COSTOTOGO_INFO_H__ 11 | #define ___CODER_COSTOTOGO_INFO_H__ 12 | /* Include files */ 13 | #include "tmwtypes.h" 14 | #include "mex.h" 15 | #include "emlrt.h" 16 | 17 | 18 | /* Function Declarations */ 19 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(void); 20 | 21 | #endif 22 | /* End of code generation (_coder_costotogo_info.h) */ 23 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/interface/_coder_costotogo_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _coder_costotogo_info.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef ___CODER_COSTOTOGO_INFO_H__ 11 | #define ___CODER_COSTOTOGO_INFO_H__ 12 | /* Include files */ 13 | #include "tmwtypes.h" 14 | #include "mex.h" 15 | #include "emlrt.h" 16 | 17 | 18 | /* Function Declarations */ 19 | extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(void); 20 | 21 | #endif 22 | /* End of code generation (_coder_costotogo_info.h) */ 23 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo_terminate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_terminate.h 3 | * 4 | * Code generation for function 'costotogo_terminate' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_TERMINATE_H__ 11 | #define __COSTOTOGO_TERMINATE_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern void costotogo_terminate(void); 22 | #endif 23 | /* End of code generation (costotogo_terminate.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/costtogo/costotogo_initialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_initialize.h 3 | * 4 | * Code generation for function 'costotogo_initialize' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_INITIALIZE_H__ 11 | #define __COSTOTOGO_INITIALIZE_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern void costotogo_initialize(void); 22 | #endif 23 | /* End of code generation (costotogo_initialize.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_terminate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_terminate.h 3 | * 4 | * Code generation for function 'costotogo_terminate' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_TERMINATE_H__ 11 | #define __COSTOTOGO_TERMINATE_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern void costotogo_terminate(void); 22 | #endif 23 | /* End of code generation (costotogo_terminate.h) */ 24 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/costotogo_initialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * costotogo_initialize.h 3 | * 4 | * Code generation for function 'costotogo_initialize' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __COSTOTOGO_INITIALIZE_H__ 11 | #define __COSTOTOGO_INITIALIZE_H__ 12 | /* Include files */ 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rtwtypes.h" 18 | #include "costotogo_types.h" 19 | 20 | /* Function Declarations */ 21 | extern void costotogo_initialize(void); 22 | #endif 23 | /* End of code generation (costotogo_initialize.h) */ 24 | -------------------------------------------------------------------------------- /config/common_params.yaml: -------------------------------------------------------------------------------- 1 | # obstacle_range: 50.0 2 | # raytrace_range: 50.0 3 | 4 | inflation_radius: 0.05 5 | 6 | # obstacle_range: 2.5 7 | # raytrace_range: 3.0 8 | 9 | max_obstacle_height: 2.0 10 | min_obstacle_height: -1.0 11 | 12 | use_dijkstra: false 13 | # visualize_potential: false 14 | 15 | # name of the frame at base-link of the spencer robot 16 | robot_base_frame: base_footprint 17 | # specifications for the footprint of the robot as polygon (in meters) 18 | #footprint: [[0.39, 0.27], [0.39, -0.27], [0.24, -0.41], [-0.24, -0.41], [-0.39, -0.27], [-0.39, 0.27], [-0.24, 0.41], [0.24, 0.41]] 19 | 20 | #footprint: [[0.756, 0.455], [0.756, -0.456], [-0.791, -0.455], [-0.791, 0.455] ] # original 21 | footprint: [[0.796, 0.495], [0.796, -0.495], [-0.831, -0.495], [-0.831, 0.495] ] # larger footprint 22 | -------------------------------------------------------------------------------- /config/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | # default parameters for testing the dwa planner from ros 2 | 3 | acc_lim_th: 3.8 4 | acc_lim_x: 3.5 5 | acc_lim_y: 0.0 6 | 7 | max_vel_x: 0.7 8 | min_vel_x: 0.0 9 | 10 | max_vel_y: 0.0 11 | min_vel_y: 0.0 12 | 13 | max_trans_vel: 0.7 14 | min_trans_vel: 0.01 15 | 16 | max_rot_vel: 0.9 17 | min_rot_vel: 0.1 18 | 19 | #sim_time: 1.0 20 | #sim_granularity: 0.05 21 | 22 | goal_distance_bias: 32.0 23 | path_distance_bias: 48.0 24 | occdist_scale: 0.05 25 | 26 | #stop_time_buffer: 0.25 27 | #oscillation_reset_dist: 0.05 28 | 29 | forward_point_distance: 0.325 30 | 31 | scaling_speed: 0.25 32 | max_scaling_factor: 0.2 33 | 34 | vx_samples: 3 35 | vy_samples: 10 36 | vtheta_samples: 20 37 | 38 | xy_goal_tolerance: 0.4 39 | yaw_goal_tolerance: 0.1 40 | 41 | rot_stopped_vel: 0.1 42 | trans_stopped_vel: 0.1 43 | -------------------------------------------------------------------------------- /include/smp/planner_utils/vertex_edge.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_VERTEX_EDGE_HPP_ 2 | #define _SMP_VERTEX_EDGE_HPP_ 3 | 4 | #include 5 | 6 | 7 | #include 8 | 9 | 10 | template< class typeparams > 11 | smp::edge< typeparams > 12 | ::edge () { 13 | 14 | vertex_src = 0; 15 | vertex_dst = 0; 16 | trajectory_edge = 0; 17 | } 18 | 19 | 20 | template< class typeparams > 21 | smp::edge< typeparams > 22 | ::~edge () { 23 | 24 | delete trajectory_edge; 25 | } 26 | 27 | 28 | template< class typeparams > 29 | smp::vertex< typeparams > 30 | ::vertex () { 31 | 32 | incoming_edges.clear(); 33 | outgoing_edges.clear(); 34 | } 35 | 36 | 37 | template< class typeparams > 38 | smp::vertex< typeparams > 39 | ::~vertex () { 40 | 41 | if (state) 42 | delete state; 43 | 44 | incoming_edges.clear(); 45 | outgoing_edges.clear(); 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /include/srl_global_planner/grid.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | 9 | //TODO: ROS PARAMETERS!!! 10 | 11 | 12 | 13 | 14 | 15 | class Grid { 16 | // Constructor and Destructor 17 | public: 18 | //TODO: use a QRectF? 19 | Grid(double x, double y, double w, double h,double cw, double ch,int nx, int ny); 20 | 21 | virtual ~Grid(); 22 | 23 | 24 | // cell occupancy 25 | bool isOccupied(double x, double y); 26 | 27 | bool isCellOccupied(int x, int y); 28 | 29 | void setOccupied(double x, double y); 30 | 31 | void unSetOccupied(double x, double y); 32 | 33 | void setCost(double x, double y, int c); 34 | 35 | int getCost(double x, double y); 36 | 37 | void clearObstacles(); 38 | 39 | // Attributes 40 | std::vector< std::vector > cells; 41 | 42 | std::vector< std::vector > cells_costs; 43 | 44 | 45 | double width; 46 | double height; 47 | double minx; 48 | double miny; 49 | double cellwidth; 50 | double cellheight; 51 | int n_cell_x; 52 | int n_cell_y; 53 | 54 | 55 | }; 56 | 57 | -------------------------------------------------------------------------------- /include/smp/common/region.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_REGION_HPP_ 2 | #define _SMP_REGION_HPP_ 3 | 4 | #include 5 | 6 | 7 | template 8 | smp::region 9 | ::region () { 10 | 11 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 12 | this->center[i] = 0.0; 13 | this->size[i] = 0.0; 14 | } 15 | } 16 | 17 | 18 | template 19 | smp::region 20 | ::~region () { 21 | 22 | 23 | } 24 | 25 | 26 | template 27 | smp::region 28 | ::region (const smp::region ®ion_in) { 29 | 30 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 31 | this->center[i] = region_in.center[i]; 32 | this->size[i] = region_in.size[i]; 33 | } 34 | } 35 | 36 | 37 | template 38 | const smp::region & 39 | smp::region 40 | ::operator=(const smp::region ®ion_in) { 41 | 42 | if (®ion_in != this) { 43 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 44 | this->center[i] = region_in.center[i]; 45 | this->size[i] = region_in.size[i]; 46 | } 47 | } 48 | 49 | return *this; 50 | } 51 | 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /include/smp/utils/branch_and_bound_base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_BRANCH_AND_BOUND_BASE_HPP_ 2 | #define _SMP_BRANCH_AND_BOUND_BASE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | template< class typeparams > 11 | smp::branch_and_bound_base 12 | ::branch_and_bound_base () { 13 | 14 | planner_bnb = NULL; 15 | 16 | upper_bound_cost = -1.0; 17 | } 18 | 19 | 20 | template< class typeparams > 21 | smp::branch_and_bound_base 22 | ::~branch_and_bound_base () { 23 | 24 | } 25 | 26 | 27 | template< class typeparams > 28 | int smp::branch_and_bound_base 29 | ::set_planner (planner_t *planner_in) { 30 | 31 | planner_bnb = planner_in; 32 | 33 | if (planner_bnb == NULL) 34 | upper_bound_cost = -1.0; 35 | 36 | return 1; 37 | } 38 | 39 | 40 | template< class typeparams > 41 | int smp::branch_and_bound_base 42 | ::set_upper_bound_cost (double upper_bound_cost_in) { 43 | 44 | if (upper_bound_cost_in <= 0.0) 45 | upper_bound_cost = -1.0; 46 | 47 | upper_bound_cost = upper_bound_cost_in; 48 | 49 | return 1; 50 | } 51 | 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /include/smp/components/cost_evaluators/time.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_COST_EVALUATOR_TIME_HPP_ 2 | #define _SMP_COST_EVALUATOR_TIME_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | 11 | using namespace std; 12 | 13 | 14 | template< class typeparams > 15 | int smp::cost_evaluator_time 16 | ::ce_update_vertex_cost (vertex_t *vertex_in) { 17 | 18 | return 1; 19 | } 20 | 21 | 22 | template< class typeparams > 23 | int smp::cost_evaluator_time 24 | ::ce_update_edge_cost (edge_t *edge_in) { 25 | 26 | return 1; 27 | } 28 | 29 | 30 | 31 | template< class typeparams > 32 | double smp::cost_evaluator_time 33 | ::evaluate_cost_trajectory (state_t *state_initial_in, 34 | trajectory_t *trajectory_in, 35 | state_t *state_final_in) { 36 | 37 | double total_time = 0.0; 38 | for (typename list::iterator iter = trajectory_in->list_inputs.begin(); 39 | iter != trajectory_in->list_inputs.end(); iter++) { 40 | 41 | input_t *input_curr = *iter; 42 | 43 | total_time += (*input_curr)[0]; 44 | 45 | } 46 | 47 | return total_time; 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /include/smp/costtogo/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rt_nonfinite.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RT_NONFINITE_H__ 11 | #define __RT_NONFINITE_H__ 12 | 13 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 14 | #include 15 | #endif 16 | #include 17 | #include "rtwtypes.h" 18 | 19 | extern real_T rtInf; 20 | extern real_T rtMinusInf; 21 | extern real_T rtNaN; 22 | extern real32_T rtInfF; 23 | extern real32_T rtMinusInfF; 24 | extern real32_T rtNaNF; 25 | extern void rt_InitInfAndNaN(size_t realSize); 26 | extern boolean_T rtIsInf(real_T value); 27 | extern boolean_T rtIsInfF(real32_T value); 28 | extern boolean_T rtIsNaN(real_T value); 29 | extern boolean_T rtIsNaNF(real32_T value); 30 | 31 | typedef struct { 32 | struct { 33 | uint32_T wordH; 34 | uint32_T wordL; 35 | } words; 36 | } BigEndianIEEEDouble; 37 | 38 | typedef struct { 39 | struct { 40 | uint32_T wordL; 41 | uint32_T wordH; 42 | } words; 43 | } LittleEndianIEEEDouble; 44 | 45 | typedef struct { 46 | union { 47 | real32_T wordLreal; 48 | uint32_T wordLuint; 49 | } wordL; 50 | } IEEESingle; 51 | 52 | #endif 53 | /* End of code generation (rt_nonfinite.h) */ 54 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rt_nonfinite.h 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | #ifndef __RT_NONFINITE_H__ 11 | #define __RT_NONFINITE_H__ 12 | 13 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 14 | #include 15 | #endif 16 | #include 17 | #include "rtwtypes.h" 18 | 19 | extern real_T rtInf; 20 | extern real_T rtMinusInf; 21 | extern real_T rtNaN; 22 | extern real32_T rtInfF; 23 | extern real32_T rtMinusInfF; 24 | extern real32_T rtNaNF; 25 | extern void rt_InitInfAndNaN(size_t realSize); 26 | extern boolean_T rtIsInf(real_T value); 27 | extern boolean_T rtIsInfF(real32_T value); 28 | extern boolean_T rtIsNaN(real_T value); 29 | extern boolean_T rtIsNaNF(real32_T value); 30 | 31 | typedef struct { 32 | struct { 33 | uint32_T wordH; 34 | uint32_T wordL; 35 | } words; 36 | } BigEndianIEEEDouble; 37 | 38 | typedef struct { 39 | struct { 40 | uint32_T wordL; 41 | uint32_T wordH; 42 | } words; 43 | } LittleEndianIEEEDouble; 44 | 45 | typedef struct { 46 | union { 47 | real32_T wordLreal; 48 | uint32_T wordLuint; 49 | } wordL; 50 | } IEEESingle; 51 | 52 | #endif 53 | /* End of code generation (rt_nonfinite.h) */ 54 | -------------------------------------------------------------------------------- /include/smp/common/regionc.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_REGIONC_HPP_ 2 | #define _SMP_REGIONC_HPP_ 3 | 4 | #include 5 | 6 | 7 | template 8 | smp::regionc 9 | ::regionc () { 10 | 11 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 12 | this->center[i] = 0.0; 13 | this->size[i] = 0.0; 14 | 15 | } 16 | this->radius=0.0; 17 | this->ang_range=0.0; 18 | } 19 | 20 | 21 | template 22 | smp::regionc 23 | ::~regionc () { 24 | 25 | 26 | } 27 | 28 | 29 | template 30 | smp::regionc 31 | ::regionc (const smp::regionc ®ion_in) { 32 | 33 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 34 | this->center[i] = region_in.center[i]; 35 | this->size[i] = region_in.size[i]; 36 | } 37 | this->radius=this->size[0]; 38 | this->ang_range=this->size[1]; 39 | } 40 | 41 | 42 | template 43 | const smp::regionc & 44 | smp::regionc 45 | ::operator=(const smp::regionc ®ion_in) { 46 | 47 | if (®ion_in != this) { 48 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 49 | this->center[i] = region_in.center[i]; 50 | this->size[i] = region_in.size[i]; 51 | } 52 | 53 | this->radius=this->size[0]; 54 | this->ang_range=this->size[1]; 55 | } 56 | 57 | return *this; 58 | } 59 | 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/smp/planner_utils/trajectory.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_TRAJECTORY_HPP_ 2 | #define _SMP_TRAJECTORY_HPP_ 3 | 4 | #include 5 | 6 | template< class typeparams > 7 | smp::trajectory< typeparams > 8 | ::trajectory () { 9 | 10 | } 11 | 12 | 13 | 14 | template< class typeparams > 15 | smp::trajectory< typeparams > 16 | ::~trajectory () { 17 | 18 | this->clear_delete (); 19 | 20 | } 21 | 22 | 23 | template< class typeparams > 24 | int smp::trajectory< typeparams > 25 | ::clear () { 26 | 27 | // Clear the list of states and the list of inputs. 28 | list_states.clear(); 29 | list_inputs.clear(); 30 | 31 | return 1; 32 | } 33 | 34 | 35 | template< class typeparams > 36 | int smp::trajectory< typeparams > 37 | ::clear_delete () { 38 | 39 | // Free all the memory occupied by the states in the list. 40 | for (typename list::iterator iter_state = list_states.begin(); 41 | iter_state != list_states.end(); iter_state++) { 42 | state_t *state_curr = *iter_state; 43 | delete state_curr; 44 | } 45 | 46 | // Free all the memory occupied by the inputs in the list. 47 | for (typename list::iterator iter_input = list_inputs.begin(); 48 | iter_input != list_inputs.end(); iter_input++) { 49 | input_t *input_curr = *iter_input; 50 | delete input_curr; 51 | } 52 | 53 | // Clear the list of states and the list of inputs. 54 | this->clear(); 55 | 56 | return 1; 57 | } 58 | 59 | 60 | 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /include/srl_global_planner/trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | Trajectory::Trajectory() 6 | { 7 | path_.clear(); 8 | drive_path_.clear(); 9 | v.clear(); 10 | w.clear(); 11 | } 12 | 13 | void Trajectory::addVelocities(double v_, double w_){ 14 | 15 | v.push_back(v_); 16 | w.push_back(w_); 17 | 18 | 19 | } 20 | void Trajectory::addPointEnd(Tpoint p) 21 | { 22 | path_.push_back(p); 23 | drive_path_.push_back(p); 24 | } 25 | 26 | void Trajectory::addPointBegin(Tpoint p) 27 | { 28 | std::cout << "Not implemented yet" << std::endl; 29 | //path_.push_front(p); 30 | } 31 | 32 | Tpoint Trajectory::getNextPoint(Tpoint p) 33 | { 34 | std::vector::iterator it = drive_path_.begin(); 35 | Tpoint n = path_[0]; 36 | 37 | int erase_point = 0; 38 | for ( ; it != drive_path_.end(); it++) 39 | { 40 | Tpoint q = (*it); 41 | double dn = sqrt((p.x-n.x)*(p.x-n.x) + (p.y-n.y)*(p.y-n.y)); 42 | double dq = sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y)); 43 | 44 | if (dq < dn) 45 | { 46 | n = q; 47 | erase_point++; 48 | } 49 | } 50 | 51 | std::cout << "Erasing: " << erase_point << std::endl; 52 | 53 | //remove it from the store 54 | drive_path_.erase(drive_path_.begin() + erase_point); 55 | 56 | return n; 57 | } 58 | 59 | 60 | void Trajectory::resample(const int new_length) 61 | { 62 | 63 | } -------------------------------------------------------------------------------- /include/smp/common/region.h: -------------------------------------------------------------------------------- 1 | /*! \file region.h 2 | \brief The standard brute-force collision checker 3 | 4 | This file implements the a region class, which defines a rectangular region 5 | in the Euclidean space, the dimension of which is a template argument. 6 | */ 7 | 8 | #ifndef _SMP_REGION_H_ 9 | #define _SMP_REGION_H_ 10 | 11 | 12 | namespace smp { 13 | 14 | //! A rectangular region in an Euclidean space of prespecified dimension. 15 | /*! 16 | This class implements a rectangular in an Euclidean space of a certain 17 | dimension given by a template parameter. 18 | */ 19 | template 20 | class region { 21 | public: 22 | 23 | //! The coordinates of the center of the region. 24 | double center[NUM_DIMENSIONS]; 25 | 26 | //! The size of the region in each dimension. 27 | double size[NUM_DIMENSIONS]; 28 | 29 | 30 | region (); 31 | ~region (); 32 | 33 | /** 34 | * \brief Copy constructor 35 | */ 36 | region (const region ®ion_in); 37 | 38 | 39 | /** 40 | * \brief Equality operator 41 | * 42 | * Two states are equal if and only if all their components are equal. This function 43 | * checks whether this criterion is satisfied. 44 | */ 45 | const region &operator=(const region ®ion_in); 46 | 47 | }; 48 | 49 | 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/remove_svn.pl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/perl -w 2 | use strict; 3 | 4 | my @rmdirs = ('.svn'); 5 | my @rmfiles = (); 6 | 7 | sub filelist 8 | { 9 | my ($dir, $regex) = @_; 10 | opendir(DIR, $dir) || return (); 11 | my @files = grep { (/$regex/) && 12 | ($_ ne '.') && ($_ ne '..') } readdir(DIR); 13 | closedir DIR; 14 | @files; 15 | } 16 | 17 | sub dirlist 18 | { 19 | my ($dir) = @_; 20 | opendir(DIR, $dir) || return (); 21 | my @dirs = grep { (-d $dir.'/'.$_) && 22 | ($_ ne '.') && ($_ ne '..') } readdir(DIR); 23 | closedir DIR; 24 | @dirs; 25 | } 26 | 27 | sub run 28 | { 29 | my ($path) = @_; 30 | $path .= '/' unless $path =~ m!\/$!; 31 | my @dirs = dirlist($path); 32 | 33 | foreach my $dir(@dirs) 34 | { 35 | 36 | my $removed = 0; 37 | foreach my $rmdir(@rmdirs) { 38 | if ($dir eq $rmdir) { 39 | print "Removing directory $path$dir\n"; 40 | `rm -rf \"$path$dir\"`; 41 | $removed = 1; last; }} 42 | run($path.$dir) unless $removed; 43 | } 44 | 45 | my @files = filelist($path, '.*'); 46 | 47 | foreach my $file(@files) 48 | { 49 | foreach my $rmfile(@rmfiles) { 50 | if ($file eq $rmfile) { 51 | print "Removing file $path$file\n"; 52 | `rm -rf \"$path$file\"`; last; }} 53 | } 54 | } 55 | 56 | if (($ARGV[0] || '') eq '--help') { 57 | print "Clean current directory. If parameter is given, ", 58 | "cleans that path instead\n"; 59 | } else { 60 | run($ARGV[0] || '.'); 61 | } 62 | -------------------------------------------------------------------------------- /include/smp/common/regionc.h: -------------------------------------------------------------------------------- 1 | /*! \file regionc.h 2 | \brief The standard brute-force collision checker 3 | 4 | 5 | */ 6 | 7 | #ifndef _SMP_REGIONC_H_ 8 | #define _SMP_REGIONC_H_ 9 | 10 | 11 | namespace smp { 12 | 13 | //! A Circular region in an Euclidean space of prespecified dimension. 14 | /*! 15 | This class implements a circular region in an Euclidean space of a certain 16 | dimension given by a template parameter. 17 | */ 18 | template 19 | class regionc { 20 | public: 21 | 22 | //! The coordinates of the center of the region. 23 | double center[NUM_DIMENSIONS]; 24 | 25 | //! The size of the region in each dimension. 26 | /// size[0] --> radius 27 | /// size[1] --> angular range 28 | /// size[2] --> to be used 29 | 30 | double size[NUM_DIMENSIONS]; 31 | double radius; 32 | double ang_range; 33 | 34 | regionc (); 35 | ~regionc (); 36 | 37 | /** 38 | * \brief Copy constructor 39 | */ 40 | regionc (const regionc ®ion_in); 41 | 42 | 43 | /** 44 | * \brief Equality operator 45 | * 46 | * Two states are equal if and only if all their components are equal. This function 47 | * checks whether this criterion is satisfied. 48 | */ 49 | const regionc &operator=(const regionc ®ion_in); 50 | 51 | }; 52 | 53 | 54 | } 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /include/smp/components/extenders/input_array_double.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_INPUT_ARRAY_DOUBLE_HPP_ 2 | #define _SMP_INPUT_ARRAY_DOUBLE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | template 11 | smp::input_array_double 12 | ::input_array_double () { 13 | 14 | for (int i = 0; i < NUM_INPUTS; i++) 15 | input_vars[i] = 0.0; 16 | 17 | } 18 | 19 | 20 | template 21 | smp::input_array_double 22 | ::input_array_double (const smp::input_array_double &input_in) { 23 | 24 | for (int i = 0; i < NUM_INPUTS; i++) 25 | input_vars[i] = input_in.input_vars[i]; 26 | 27 | } 28 | 29 | 30 | template 31 | smp::input_array_double 32 | ::~input_array_double () { 33 | 34 | } 35 | 36 | 37 | template 38 | const smp::input_array_double & 39 | smp::input_array_double 40 | ::operator=(const smp::input_array_double &input_in) { 41 | 42 | if (&input_in != this) { 43 | for (int i = 0; i < NUM_INPUTS; i++) 44 | input_vars[i] = input_in.input_vars[i]; 45 | } 46 | 47 | return *this; 48 | } 49 | 50 | template 51 | double & 52 | smp::input_array_double 53 | ::operator[] (int index_in) { 54 | 55 | if ( (index_in < NUM_INPUTS) && (index_in >= 0) ) 56 | return input_vars[index_in]; 57 | 58 | // TODO: ideally throw an exception also. 59 | return input_vars[0]; 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /include/smp/components/extenders/state_array_double.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_STATE_ARRAY_DOUBLE_HPP_ 2 | #define _SMP_STATE_ARRAY_DOUBLE_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | template 11 | smp::state_array_double 12 | ::state_array_double () { 13 | 14 | for (int i = 0; i < NUM_STATES; i++) 15 | state_vars[i] = 0.0; 16 | 17 | } 18 | 19 | 20 | template 21 | smp::state_array_double 22 | ::state_array_double (const smp::state_array_double &state_in) { 23 | 24 | for (int i = 0; i < NUM_STATES; i++) 25 | state_vars[i] = state_in.state_vars[i]; 26 | 27 | } 28 | 29 | 30 | template 31 | smp::state_array_double 32 | ::~state_array_double () { 33 | 34 | } 35 | 36 | 37 | template 38 | const smp::state_array_double & 39 | smp::state_array_double 40 | ::operator=(const smp::state_array_double &state_in) { 41 | 42 | if (&state_in != this) { 43 | for (int i = 0; i < NUM_STATES; i++) 44 | state_vars[i] = state_in.state_vars[i]; 45 | } 46 | 47 | return *this; 48 | } 49 | 50 | template 51 | double& 52 | smp::state_array_double 53 | ::operator[] (int index_in) { 54 | 55 | if ( (index_in < NUM_STATES) && (index_in >= 0) ) 56 | return state_vars[index_in]; 57 | 58 | // TODO: ideally throw an exception also. 59 | return state_vars[0]; 60 | } 61 | 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/tansig_apply122_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
 
Order
Variable
Type
Size
Class
Complex
1aOutput4 x 1doubleNo
2nInput4 x 1doubleNo
30 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/tansig_apply95_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
 
Order
Variable
Type
Size
Class
Complex
1aOutput14 x 1doubleNo
2nInput14 x 1doubleNo
30 | -------------------------------------------------------------------------------- /include/smp/planners/base_incremental.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_PLANNER_INCREMENTAL_HPP_ 2 | #define _SMP_PLANNER_INCREMENTAL_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | 9 | template< class typeparams > 10 | smp::planner_incremental 11 | ::planner_incremental () { 12 | 13 | root_vertex = 0; 14 | } 15 | 16 | 17 | template< class typeparams > 18 | smp::planner_incremental 19 | ::~planner_incremental () { 20 | 21 | // Note that the root vertex is deleted by the smp_planner class 22 | } 23 | 24 | template< class typeparams > 25 | smp::planner_incremental 26 | ::planner_incremental (sampler_t &sampler_in, distance_evaluator_t &distance_evaluator_in, extender_t &extender_in, 27 | collision_checker_t &collision_checker_in, model_checker_t &model_checker_in) 28 | : planner (sampler_in, distance_evaluator_in, extender_in, collision_checker_in, model_checker_in) { 29 | 30 | } 31 | 32 | 33 | template< class typeparams > 34 | int smp::planner_incremental 35 | ::initialize (state_t *initial_state_in) { 36 | 37 | planner_t::initialize(); // This function deletes all existing vertices 38 | // in the graph, including the root vertex. 39 | 40 | if (initial_state_in == 0) { 41 | root_vertex = 0; 42 | return 1; 43 | } 44 | 45 | root_vertex = new vertex_t; 46 | root_vertex->state = initial_state_in; 47 | 48 | this->insert_vertex (root_vertex); 49 | 50 | return 1; 51 | } 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/tansig_apply122_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
 
Order
Variable
Type
Size
Class
Complex
1aOutput4 x 1doubleNo
2nInput4 x 1doubleNo
30 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/tansig_apply95_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
 
Order
Variable
Type
Size
Class
Complex
1aOutput14 x 1doubleNo
2nInput14 x 1doubleNo
30 | -------------------------------------------------------------------------------- /config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | # use rolling window version of costmap, as local costmap is not static 2 | rolling_window: true 3 | # local map will be publish at modrate frequency of 10 4 | publish_frequency: 10.0 5 | # width of the map in meters 6 | width: 5.0 7 | # height of the map in meters 8 | height: 5.0 9 | # resolution of the map in meters/cell 10 | resolution: 2.5 11 | # local costmap will operate in the map frame, transform between this and robot-base frame must be available 12 | global_frame: map 13 | 14 | static_map: true 15 | 16 | # name of the frame at base-link of the spencer robot 17 | robot_base_frame: base_link 18 | # specifications for the footprint of the robot as polygon (in meters) 19 | # footprint: [ [0.756, 0.455], [0.756, -0.455], [-0.791, -0.455], [-0.791, 0.455] ] # real footprint 20 | footprint: [[0.796, 0.495], [0.796, -0.495], [-0.831, -0.495], [-0.831, 0.495] ] # larger footprint 21 | 22 | #footprint: [[0.39, 0.27], [0.39, -0.27], [0.24, -0.41], [-0.24, -0.41], [-0.39, -0.27], [-0.39, 0.27], [-0.24, 0.41], [0.24, 0.41]] 23 | # for local plannign we will use only obstacles, inflation layers 24 | plugins: 25 | - {name: static_map, type: "costmap_2d::StaticLayer"} 26 | # - {name: inflater, type: "costmap_2d::InflationLayer"} 27 | # sources for obstacle observation and their data types 28 | # obstacles: 29 | # observation_sources: /spencer/sensors/laser_front/echo0 30 | # /spencer/sensors/laser_front/echo0: {data_type: LaserScan, sensor_frame: /laser_front_link, marking: true, clearing: true, obstacle_range: 6, raytrace_range: 2} 31 | # track_unknown_space: true 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Social Robotics Lab, University of Freiburg 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of srl_global_planner nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /include/srl_global_planner/srl_trajectory.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_H 2 | #define TRAJECTORY_H 3 | 4 | #include 5 | 6 | /// \brief simple point in a trajectory 7 | typedef struct TrajectoryPoint { 8 | double x; 9 | double y; 10 | double z; 11 | 12 | TrajectoryPoint() { x = 0.0; y = 0.0; z = 0.0; } 13 | TrajectoryPoint(double xx, double yy) : x(xx), y(yy) { z = 0.0; } 14 | TrajectoryPoint(double xx, double yy, double zz) : x(xx), y(yy), z(zz) { } 15 | TrajectoryPoint(double* p) { x = p[0]; y = p[1]; z = p[2]; } 16 | 17 | TrajectoryPoint& operator=(TrajectoryPoint const& copy) 18 | { 19 | x = copy.x; 20 | y = copy.y; 21 | z = copy.z; 22 | return *this; 23 | } 24 | 25 | } Tpoint; 26 | 27 | 28 | /// \class Trajectory 29 | /// \brief A list of point for the robot agent to follow 30 | class Trajectory { 31 | 32 | public: 33 | 34 | 35 | Trajectory(); 36 | 37 | ~Trajectory() { path_.clear();} 38 | 39 | 40 | void addPointEnd(Tpoint p); 41 | 42 | void addVelocities(double v_, double w_); 43 | 44 | void addPointBegin(Tpoint p); 45 | 46 | int length() {return path_.size(); } 47 | 48 | void resample(const int new_length); 49 | 50 | void reset() { path_.clear(); v.clear(); w.clear();} 51 | 52 | std::vector getPath() {return path_;} 53 | 54 | 55 | 56 | 57 | 58 | std::vector v; 59 | std::vector w; 60 | 61 | Tpoint getNextPoint(Tpoint p); 62 | 63 | private: 64 | 65 | std::vector path_; 66 | std::vector drive_path_; 67 | 68 | 69 | }; 70 | 71 | #endif // TRAJECTORY_H 72 | -------------------------------------------------------------------------------- /include/smp/planners/planner_parameters.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_PLANNER_PARAMETERS_HPP_ 2 | #define _SMP_PLANNER_PARAMETERS_HPP_ 3 | 4 | #include 5 | 6 | 7 | smp::planner_parameters 8 | ::planner_parameters () { 9 | 10 | phase = 2; 11 | 12 | gamma = 20.0; 13 | dimension = 3; 14 | max_radius = 10.0; 15 | fixed_radius = -1.0; 16 | } 17 | 18 | 19 | smp::planner_parameters 20 | ::~planner_parameters () { 21 | 22 | } 23 | 24 | 25 | int smp::planner_parameters 26 | ::set_phase (int phase_in) { 27 | 28 | if ( (0 <= phase_in) && (phase_in <= 2) ) { 29 | phase = phase_in; 30 | return 1; 31 | } 32 | else { 33 | return 0; 34 | } 35 | } 36 | 37 | 38 | int smp::planner_parameters 39 | ::set_gamma (double gamma_in) { 40 | 41 | if (0.0 < gamma_in) { 42 | gamma = gamma_in; 43 | return 1; 44 | } 45 | else { 46 | return 0; 47 | } 48 | } 49 | 50 | 51 | int smp::planner_parameters 52 | ::set_dimension (int dimension_in) { 53 | 54 | if (2 <= dimension_in) { 55 | dimension = dimension_in; 56 | return 1; 57 | } 58 | else { 59 | return 0; 60 | } 61 | } 62 | 63 | 64 | int smp::planner_parameters 65 | ::set_max_radius (double max_radius_in) { 66 | 67 | if (0 < max_radius_in) { 68 | max_radius = max_radius_in; 69 | return 1; 70 | } 71 | else { 72 | return 0; 73 | } 74 | } 75 | 76 | 77 | int smp::planner_parameters 78 | ::set_fixed_radius (double fixed_radius_in) { 79 | 80 | if (fixed_radius_in > 0.0) 81 | fixed_radius = fixed_radius_in; 82 | else 83 | fixed_radius = -1.0; 84 | 85 | return 1; 86 | } 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /include/smp/components/cost_evaluators/time.h: -------------------------------------------------------------------------------- 1 | /*! \file components/cost_evaluators/time.h 2 | \brief The cost evaluator based on the execution time 3 | 4 | This file implements the smp_cost_evaluator_time class that computes the 5 | cost of the trajectory based on the time it takes to execute it. 6 | */ 7 | 8 | #ifndef _SMP_COST_EVALUATOR_TIME_H_ 9 | #define _SMP_COST_EVALUATOR_TIME_H_ 10 | 11 | #include 12 | 13 | 14 | namespace smp { 15 | 16 | //! The cost evaluator class based on the trajectory execution time 17 | /*! 18 | This class computes the cost of a trajectory according to the time 19 | it takes to execute that particular trajectory. 20 | 21 | \ingroup cost_evaluators 22 | */ 23 | template< class typeparams > 24 | class cost_evaluator_time : public cost_evaluator_base { 25 | 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::input input_t; 29 | typedef typename typeparams::vertex_data vertex_data_t; 30 | typedef typename typeparams::edge_data edge_data_t; 31 | 32 | typedef vertex vertex_t; 33 | typedef edge edge_t; 34 | 35 | typedef trajectory trajectory_t; 36 | 37 | 38 | public: 39 | 40 | int ce_update_vertex_cost (vertex_t *vertex_in); 41 | 42 | int ce_update_edge_cost (edge_t *edge_in); 43 | 44 | double evaluate_cost_trajectory (state_t *state_initial_in, 45 | trajectory_t *trajectory_in, 46 | state_t *state_final_in = 0); 47 | 48 | }; 49 | 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/tansig_apply95_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 67
 3 | 68% Sigmoid Symmetric Transfer Function
 4 | 69function a = tansig_apply(n)
 5 | 70  a = 2 ./ (1 + exp(-2*n)) - 1;
 6 | 71end
 7 | 72
 8 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/tansig_apply122_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 67
 3 | 68% Sigmoid Symmetric Transfer Function
 4 | 69function a = tansig_apply(n)
 5 | 70  a = 2 ./ (1 + exp(-2*n)) - 1;
 6 | 71end
 7 | 72
 8 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/tansig_apply95_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 67
 3 | 68% Sigmoid Symmetric Transfer Function
 4 | 69function a = tansig_apply(n)
 5 | 70  a = 2 ./ (1 + exp(-2*n)) - 1;
 6 | 71end
 7 | 72
 8 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/tansig_apply122_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 67
 3 | 68% Sigmoid Symmetric Transfer Function
 4 | 69function a = tansig_apply(n)
 5 | 70  a = 2 ./ (1 + exp(-2*n)) - 1;
 6 | 71end
 7 | 72
 8 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/external_libraries/halton/halton.h: -------------------------------------------------------------------------------- 1 | double arc_cosine ( double c ); 2 | double atan4 ( double y, double x ); 3 | char digit_to_ch ( int i ); 4 | int get_seed ( void ); 5 | bool halham_leap_check ( int dim_num, int leap[] ); 6 | bool halham_n_check ( int n ); 7 | bool halham_dim_num_check ( int dim_num ); 8 | bool halham_seed_check ( int dim_num, int seed[] ); 9 | bool halham_step_check ( int step ); 10 | void halham_write ( int dim_num, int n, int step, int seed[], int leap[], int base[], 11 | double r[], char *file_out_name ); 12 | void halton ( double r[] ); 13 | bool halton_base_check ( int dim_num, int base[] ); 14 | int *halton_base_get ( void ); 15 | void halton_base_set ( int base[] ); 16 | int *halton_leap_get ( void ); 17 | void halton_leap_set ( int leap[] ); 18 | int halton_dim_num_get ( void ); 19 | void halton_dim_num_set ( int dim_num ); 20 | int *halton_seed_get ( void ); 21 | void halton_seed_set ( int seed[] ); 22 | void halton_sequence ( int n, double r[] ); 23 | int halton_step_get ( void ); 24 | void halton_step_set ( int step ); 25 | int i4_log_10 ( int i ); 26 | int i4_min ( int i1, int i2 ); 27 | void i4_to_halton ( int dim_num, int step, int seed[], int leap[], int base[], 28 | double r[] ); 29 | void i4_to_halton_sequence ( int dim_num, int n, int step, int seed[], int leap[], 30 | int base[], double r[] ); 31 | char *i4_to_s ( int i ); 32 | void i4vec_transpose_print ( int n, int a[], char *title ); 33 | int prime ( int n ); 34 | double r8_epsilon ( void ); 35 | double r8vec_dot_product ( int n, double *r1, double *r2 ); 36 | double r8vec_norm_l2 ( int n, double a[] ); 37 | int s_len_trim ( char *s ); 38 | void timestamp ( void ); 39 | char *timestring ( void ); 40 | void u1_to_sphere_unit_2d ( double u[1], double x[2] ); 41 | void u2_to_ball_unit_2d ( double u[2], double x[2] ); 42 | void u2_to_sphere_unit_3d ( double u[2], double x[3] ); 43 | void u3_to_ball_unit_3d ( double u[3], double x[3] ); 44 | -------------------------------------------------------------------------------- /include/smp/planner_utils/trajectory.h: -------------------------------------------------------------------------------- 1 | /*! \file trajectory.h 2 | \brief Definition of the trajectory class 3 | 4 | */ 5 | 6 | #ifndef _SMP_TRAJECTORY_H_ 7 | #define _SMP_TRAJECTORY_H_ 8 | 9 | #include 10 | 11 | using namespace std; 12 | 13 | 14 | namespace smp { 15 | 16 | //! Trajectory definition as a states with interleaving inputs. 17 | /*! 18 | The trajectory class, composed of a list of states and a list of inputs, 19 | is an implementation of the notion of a trajectory that connects two given 20 | states in the graph. 21 | 22 | \ingroup graphs 23 | */ 24 | template< class typeparams > 25 | class trajectory { 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::input input_t; 29 | 30 | public: 31 | 32 | //! A list of the states in the trajectory. 33 | list< state_t* > list_states; 34 | 35 | //! A list of the inputs in the trajectory. 36 | list< input_t* > list_inputs; 37 | 38 | trajectory (); 39 | ~trajectory (); 40 | 41 | //! Clears the trajectory. 42 | /*! 43 | This function clears both the state list and the input list in the trajectory. 44 | But, it does NOT attempt to free the memory occupied by the said states and 45 | inputs. 46 | */ 47 | int clear (); 48 | 49 | //! Clears the trajectory and frees the memory. 50 | /*! 51 | This function clears both the state list and the input list in the trajectory. 52 | It also frees the memory occupied by the said states and the inputs, by calling 53 | the delete operator with each state and input present in the lists. 54 | */ 55 | int clear_delete (); 56 | 57 | // TODO: this may require some interface since it is given to user functions as a parameter. 58 | }; 59 | 60 | 61 | } 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /include/smp/interfaces/base.h: -------------------------------------------------------------------------------- 1 | /*! \file interfaces/base.h 2 | \brief The abstract interfacer 3 | 4 | This file provides the necessary classes and function to interface 5 | certain robotics libraries, e.g., libbot, Microsoft Robotics, and the Robot 6 | Operating System (ROS), and their visualization tools. 7 | */ 8 | 9 | #ifndef _SMP_INTERFACE_BASE_H_ 10 | #define _SMP_INTERFACE_BASE_H_ 11 | 12 | 13 | #include 14 | 15 | 16 | namespace smp { 17 | 18 | //! The abstract class that specifies the structure of a interfacing component. 19 | /*! 20 | The interfacing component provides a function to publish the graph that is 21 | stored in the planner. It also provides a function to visualize a certain 22 | trajectory (for instance, this trajectory can be the optimal trajectory in 23 | the graph). 24 | 25 | \ingroup interfaces 26 | */ 27 | template< class typeparams > 28 | class interface_base { 29 | 30 | typedef trajectory trajectory_t; 31 | typedef planner planner_t; 32 | 33 | protected: 34 | 35 | 36 | public: 37 | 38 | /** 39 | * \brief Publishes the graph maintained by the planner 40 | * 41 | * This function publishes the graph maintained by the planner. 42 | * 43 | * @returns Returns 1 for success, and a non-positive value to indicate an error. 44 | */ 45 | virtual int publish_data () = 0; 46 | 47 | 48 | /** 49 | * \brief Publishes a given trajectory 50 | * 51 | * This function will send out the messages (of the development environment) 52 | * containing the trajectory given as an argument. 53 | * 54 | * @param trajectory The trajectory that will be published. 55 | * 56 | * @returns Returns 1 for success, and a non-positive value to indicate an error. 57 | */ 58 | virtual int publish_trajectory (trajectory_t &trajectory) = 0; 59 | }; 60 | 61 | 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/mapminmax_reverse157_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 72
 3 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 4 | 74function x = mapminmax_reverse(y,settings_gain,settings_xoffset,settings_ymin)
 5 | 75  x = bsxfun(@minus,y,settings_ymin);
 6 | 76  x = bsxfun(@rdivide,x,settings_gain);
 7 | 77  x = bsxfun(@plus,x,settings_xoffset);
 8 | 78end
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/mapminmax_reverse157_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 72
 3 | 73% Map Minimum and Maximum Output Reverse-Processing Function
 4 | 74function x = mapminmax_reverse(y,settings_gain,settings_xoffset,settings_ymin)
 5 | 75  x = bsxfun(@minus,y,settings_ymin);
 6 | 76  x = bsxfun(@rdivide,x,settings_gain);
 7 | 77  x = bsxfun(@plus,x,settings_xoffset);
 8 | 78end
 9 |
10 | -------------------------------------------------------------------------------- /include/smp/components/extenders/input_array_double.h: -------------------------------------------------------------------------------- 1 | /*! \file /components/extenders/input_array_double.h 2 | \brief An implementation of an input data structure as a double array. 3 | 4 | This file includes a class that provides the implementation of the input 5 | data structure as a double array. 6 | */ 7 | 8 | #ifndef _SMP_INPUT_ARRAY_DOUBLE_H_ 9 | #define _SMP_INPUT_ARRAY_DOUBLE_H_ 10 | 11 | 12 | namespace smp { 13 | 14 | //! Implementation of the input data structure as a double array. 15 | /*! 16 | This class implements the input data structure as a double array. The 17 | dimension of the array is a template parameter to the class. 18 | 19 | \ingroup inputs 20 | */ 21 | template 22 | class input_array_double { 23 | 24 | public: 25 | 26 | //! Input variables array. 27 | /*! 28 | The implementation of the input variables as a double array 29 | of size NUM_INPUTS, which is the template argument for this class. 30 | */ 31 | double input_vars[NUM_INPUTS]; 32 | 33 | input_array_double (); 34 | ~input_array_double (); 35 | 36 | 37 | /** 38 | * \brief The copy constructor 39 | */ 40 | input_array_double (const input_array_double &input_in); 41 | 42 | 43 | /** 44 | * \brief The equality operator 45 | * 46 | * Two inputs are equal if and only if all their components are equal. This function 47 | * checks whether this criterion is satisfied. 48 | */ 49 | const input_array_double &operator=(const input_array_double &input_in); 50 | 51 | 52 | /** 53 | * \brief The bracket operator that returns the given element from the array. 54 | * 55 | * The bracket operator returns a reference to the indexed element in the array. 56 | * 57 | * @param index_in The index of the input variable. 58 | * 59 | * @returns Returns a reference to the input variable with index index_in. 60 | */ 61 | inline double& operator[] (int index_in); 62 | 63 | }; 64 | 65 | 66 | } 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /include/smp/components/extenders/state_array_double.h: -------------------------------------------------------------------------------- 1 | /*! \file /components/extenders/state_array_double.h 2 | \brief An implementation of an state data structure as a double array. 3 | 4 | This file includes a class that provides the implementation of the state 5 | data structure as a double array. 6 | */ 7 | 8 | #ifndef _SMP_STATE_ARRAY_DOUBLE_H_ 9 | #define _SMP_STATE_ARRAY_DOUBLE_H_ 10 | 11 | 12 | namespace smp { 13 | 14 | //! Implementation of the state data structure as a double array. 15 | /*! 16 | This class implements the state data structure as a double array. The 17 | dimension of the array is a template parameter to the class. 18 | 19 | \ingroup states 20 | */ 21 | template 22 | class state_array_double { 23 | 24 | public: 25 | 26 | //! State variables array. 27 | /*! 28 | The implementation of the state variables as a double array 29 | of size NUM_INPUTS, which is the template argument for this class. 30 | */ 31 | double state_vars[NUM_STATES]; 32 | 33 | state_array_double (); 34 | ~state_array_double (); 35 | 36 | 37 | /** 38 | * \brief Copy constructor 39 | */ 40 | state_array_double (const state_array_double &state_in); 41 | 42 | 43 | /** 44 | * \brief Equality operator 45 | * 46 | * Two states are equal if and only if all their components are equal. This function 47 | * checks whether this criterion is satisfied. 48 | */ 49 | const state_array_double &operator=(const state_array_double &state_in); 50 | 51 | 52 | /** 53 | * \brief The bracket operator that returns the given element from the array. 54 | * 55 | * The bracket operator returns a reference to the indexed element in the array. 56 | * 57 | * @param index_in The index of the state variable. 58 | * 59 | * @returns Returns a reference to the state variable with index index_in. 60 | */ 61 | inline double& operator[] (int index_in); 62 | 63 | 64 | }; 65 | 66 | 67 | } 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srl_global_planner 4 | 0.0.1 5 | The srl_global_planner package 6 | 7 | 8 | 9 | 10 | palmieri 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | tf 26 | visualization_msgs 27 | cmake_modules 28 | pluginlib 29 | costmap_2d 30 | nav_core 31 | spencer_tracking_msgs 32 | spencer_nav_msgs 33 | spencer_control_msgs 34 | navfn 35 | global_planner 36 | 37 | navfn 38 | global_planner 39 | spencer_tracking_msgs 40 | spencer_nav_msgs 41 | spencer_control_msgs 42 | nav_core 43 | costmap_2d 44 | pluginlib 45 | cmake_modules 46 | geometry_msgs 47 | nav_msgs 48 | roscpp 49 | rospy 50 | std_msgs 51 | tf 52 | visualization_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/mapminmax_reverse157_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 |
 
Order
Variable
Type
Size
Class
Complex
1xOutput1 x 1doubleNo
2yInput1 x 1doubleNo
3settings_gainInput1 x 1doubleNo
4settings_xoffsetInput1 x 1doubleNo
5settings_yminInput1 x 1doubleNo
51 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/mapminmax_apply47_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 |
 
Order
Variable
Type
Size
Class
Complex
1yOutput14 x 1doubleNo
2xInput14 x 1doubleNo
3settings_gainInput14 x 1doubleNo
4settings_xoffsetInput14 x 1doubleNo
5settings_yminInput1 x 1doubleNo
51 | -------------------------------------------------------------------------------- /include/smp/costtogo/rt_nonfinite.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rt_nonfinite.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finites, 13 | * (Inf, NaN and -Inf). 14 | */ 15 | #include "rt_nonfinite.h" 16 | #include "rtGetNaN.h" 17 | #include "rtGetInf.h" 18 | 19 | real_T rtInf; 20 | real_T rtMinusInf; 21 | real_T rtNaN; 22 | real32_T rtInfF; 23 | real32_T rtMinusInfF; 24 | real32_T rtNaNF; 25 | 26 | /* Function: rt_InitInfAndNaN ================================================== 27 | * Abstract: 28 | * Initialize the rtInf, rtMinusInf, and rtNaN needed by the 29 | * generated code. NaN is initialized as non-signaling. Assumes IEEE. 30 | */ 31 | void rt_InitInfAndNaN(size_t realSize) 32 | { 33 | (void) (realSize); 34 | rtNaN = rtGetNaN(); 35 | rtNaNF = rtGetNaNF(); 36 | rtInf = rtGetInf(); 37 | rtInfF = rtGetInfF(); 38 | rtMinusInf = rtGetMinusInf(); 39 | rtMinusInfF = rtGetMinusInfF(); 40 | } 41 | 42 | /* Function: rtIsInf ================================================== 43 | * Abstract: 44 | * Test if value is infinite 45 | */ 46 | boolean_T rtIsInf(real_T value) 47 | { 48 | return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); 49 | } 50 | 51 | /* Function: rtIsInfF ================================================= 52 | * Abstract: 53 | * Test if single-precision value is infinite 54 | */ 55 | boolean_T rtIsInfF(real32_T value) 56 | { 57 | return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); 58 | } 59 | 60 | /* Function: rtIsNaN ================================================== 61 | * Abstract: 62 | * Test if value is not a number 63 | */ 64 | boolean_T rtIsNaN(real_T value) 65 | { 66 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 67 | return _isnan(value)? TRUE:FALSE; 68 | #else 69 | return (value!=value)? 1U:0U; 70 | #endif 71 | } 72 | 73 | /* Function: rtIsNaNF ================================================= 74 | * Abstract: 75 | * Test if single-precision value is not a number 76 | */ 77 | boolean_T rtIsNaNF(real32_T value) 78 | { 79 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 80 | return _isnan((real_T)value)? true:false; 81 | #else 82 | return (value!=value)? 1U:0U; 83 | #endif 84 | } 85 | 86 | 87 | /* End of code generation (rt_nonfinite.c) */ 88 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/mapminmax_reverse157_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 |
 
Order
Variable
Type
Size
Class
Complex
1xOutput1 x 1doubleNo
2yInput1 x 1doubleNo
3settings_gainInput1 x 1doubleNo
4settings_xoffsetInput1 x 1doubleNo
5settings_yminInput1 x 1doubleNo
51 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/mapminmax_apply47_watch.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 |
 
Order
Variable
Type
Size
Class
Complex
1yOutput14 x 1doubleNo
2xInput14 x 1doubleNo
3settings_gainInput14 x 1doubleNo
4settings_xoffsetInput14 x 1doubleNo
5settings_yminInput1 x 1doubleNo
51 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rt_nonfinite.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rt_nonfinite.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finites, 13 | * (Inf, NaN and -Inf). 14 | */ 15 | #include "rt_nonfinite.h" 16 | #include "rtGetNaN.h" 17 | #include "rtGetInf.h" 18 | 19 | real_T rtInf; 20 | real_T rtMinusInf; 21 | real_T rtNaN; 22 | real32_T rtInfF; 23 | real32_T rtMinusInfF; 24 | real32_T rtNaNF; 25 | 26 | /* Function: rt_InitInfAndNaN ================================================== 27 | * Abstract: 28 | * Initialize the rtInf, rtMinusInf, and rtNaN needed by the 29 | * generated code. NaN is initialized as non-signaling. Assumes IEEE. 30 | */ 31 | void rt_InitInfAndNaN(size_t realSize) 32 | { 33 | (void) (realSize); 34 | rtNaN = rtGetNaN(); 35 | rtNaNF = rtGetNaNF(); 36 | rtInf = rtGetInf(); 37 | rtInfF = rtGetInfF(); 38 | rtMinusInf = rtGetMinusInf(); 39 | rtMinusInfF = rtGetMinusInfF(); 40 | } 41 | 42 | /* Function: rtIsInf ================================================== 43 | * Abstract: 44 | * Test if value is infinite 45 | */ 46 | boolean_T rtIsInf(real_T value) 47 | { 48 | return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); 49 | } 50 | 51 | /* Function: rtIsInfF ================================================= 52 | * Abstract: 53 | * Test if single-precision value is infinite 54 | */ 55 | boolean_T rtIsInfF(real32_T value) 56 | { 57 | return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); 58 | } 59 | 60 | /* Function: rtIsNaN ================================================== 61 | * Abstract: 62 | * Test if value is not a number 63 | */ 64 | boolean_T rtIsNaN(real_T value) 65 | { 66 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 67 | return _isnan(value)? TRUE:FALSE; 68 | #else 69 | return (value!=value)? 1U:0U; 70 | #endif 71 | } 72 | 73 | /* Function: rtIsNaNF ================================================= 74 | * Abstract: 75 | * Test if single-precision value is not a number 76 | */ 77 | boolean_T rtIsNaNF(real32_T value) 78 | { 79 | #if defined(_MSC_VER) && (_MSC_VER <= 1200) 80 | return _isnan((real_T)value)? true:false; 81 | #else 82 | return (value!=value)? 1U:0U; 83 | #endif 84 | } 85 | 86 | 87 | /* End of code generation (rt_nonfinite.c) */ 88 | -------------------------------------------------------------------------------- /include/smp/external_libraries/inc_mu_mc/pt.h: -------------------------------------------------------------------------------- 1 | #ifndef __MU_CALCULUS_INCREMENTAL_MODEL_CHECKER_PT_ 2 | #define __MU_CALCULUS_INCREMENTAL_MODEL_CHECKER_PT_ 3 | 4 | 5 | // Parse Tree 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | enum PT_type { 17 | PT_PRP = 1, 18 | PT_VAR, 19 | PT_LFP, 20 | PT_GFP, 21 | PT_SUC, 22 | PT_AND, 23 | PT_OR 24 | }; 25 | 26 | class PT_node; 27 | class PT_prp; 28 | class PT_var; 29 | class PT_operator; 30 | 31 | typedef set subformulaeSet; 32 | typedef set::iterator subformulaeSet_it; 33 | 34 | typedef set propositionSet; 35 | typedef set::iterator propositionSet_it; 36 | 37 | class PT_node { 38 | public: 39 | PT_type type; 40 | PT_node *parent; 41 | virtual subformulaeSet getChildren(); 42 | void printType (); 43 | }; 44 | 45 | class PT_prp:public PT_node { 46 | public: 47 | int prp; 48 | virtual subformulaeSet getChildren(); 49 | }; 50 | 51 | class PT_var:public PT_node { 52 | public: 53 | int var; 54 | virtual subformulaeSet getChildren(); 55 | }; 56 | 57 | class PT_operator:public PT_node { 58 | public: 59 | subformulaeSet children; 60 | int boundVar; 61 | virtual subformulaeSet getChildren(); 62 | }; 63 | 64 | class ParseTree { 65 | public: 66 | ParseTree (); 67 | ~ParseTree (); 68 | int parseFormula (string s); 69 | bool isEmpty (); 70 | // subformulaeSet& getSubformulaeSuc (); 71 | // subformulaeSet& getSubformulaeMu (); 72 | // subformulaeSet& getSubformulaeNu (); 73 | // void SearchBranchForType (PT_node *node, subformulaeSet &sfSet, PT_type nodeType); 74 | 75 | PT_node *getRoot (); 76 | PT_node *getBoundFormula (PT_node *node_var); 77 | 78 | // Debug/Test/Visualization related functions 79 | void printParseTree (PT_node *ptnode); 80 | 81 | bool compareFormulaSize (PT_node *ptnode_a, PT_node *ptnode_b); // True if ptnode_a < ptnode_b in the parse tree 82 | 83 | // The following two functions are not used anymore... 84 | // bool booleanCheck (PT_node *root, subformulaeSet &satisfiedSFThis, 85 | // subformulaeSet &satisfiedSFNext, propositionSet &satisfiedPrp); 86 | // bool fixedpointCheck (PT_node *root, subformulaeSet &satisfiedSFThis, 87 | // subformulaeSet &satisfiedSFNext, propositionSet &satisfiedPrp, int var, bool lfp); 88 | 89 | private: 90 | PT_node *root; 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /include/smp/costtogo/rtGetNaN.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetNaN.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finite, NaN 13 | */ 14 | #include "rtGetNaN.h" 15 | #define NumBitsPerChar 8U 16 | 17 | /* Function: rtGetNaN ================================================== 18 | * Abstract: 19 | * Initialize rtNaN needed by the generated code. 20 | * NaN is initialized as non-signaling. Assumes IEEE. 21 | */ 22 | real_T rtGetNaN(void) 23 | { 24 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 25 | real_T nan = 0.0; 26 | if (bitsPerReal == 32U) { 27 | nan = rtGetNaNF(); 28 | } else { 29 | uint16_T one = 1U; 30 | enum { 31 | LittleEndian, 32 | BigEndian 33 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 34 | switch (machByteOrder) { 35 | case LittleEndian: 36 | { 37 | union { 38 | LittleEndianIEEEDouble bitVal; 39 | real_T fltVal; 40 | } tmpVal; 41 | 42 | tmpVal.bitVal.words.wordH = 0xFFF80000U; 43 | tmpVal.bitVal.words.wordL = 0x00000000U; 44 | nan = tmpVal.fltVal; 45 | break; 46 | } 47 | 48 | case BigEndian: 49 | { 50 | union { 51 | BigEndianIEEEDouble bitVal; 52 | real_T fltVal; 53 | } tmpVal; 54 | 55 | tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; 56 | tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; 57 | nan = tmpVal.fltVal; 58 | break; 59 | } 60 | } 61 | } 62 | 63 | return nan; 64 | } 65 | 66 | /* Function: rtGetNaNF ================================================== 67 | * Abstract: 68 | * Initialize rtNaNF needed by the generated code. 69 | * NaN is initialized as non-signaling. Assumes IEEE. 70 | */ 71 | real32_T rtGetNaNF(void) 72 | { 73 | IEEESingle nanF = { { 0 } }; 74 | uint16_T one = 1U; 75 | enum { 76 | LittleEndian, 77 | BigEndian 78 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 79 | switch (machByteOrder) { 80 | case LittleEndian: 81 | { 82 | nanF.wordL.wordLuint = 0xFFC00000U; 83 | break; 84 | } 85 | 86 | case BigEndian: 87 | { 88 | nanF.wordL.wordLuint = 0x7FFFFFFFU; 89 | break; 90 | } 91 | } 92 | 93 | return nanF.wordL.wordLreal; 94 | } 95 | 96 | /* End of code generation (rtGetNaN.c) */ 97 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rtGetNaN.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetNaN.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finite, NaN 13 | */ 14 | #include "rtGetNaN.h" 15 | #define NumBitsPerChar 8U 16 | 17 | /* Function: rtGetNaN ================================================== 18 | * Abstract: 19 | * Initialize rtNaN needed by the generated code. 20 | * NaN is initialized as non-signaling. Assumes IEEE. 21 | */ 22 | real_T rtGetNaN(void) 23 | { 24 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 25 | real_T nan = 0.0; 26 | if (bitsPerReal == 32U) { 27 | nan = rtGetNaNF(); 28 | } else { 29 | uint16_T one = 1U; 30 | enum { 31 | LittleEndian, 32 | BigEndian 33 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 34 | switch (machByteOrder) { 35 | case LittleEndian: 36 | { 37 | union { 38 | LittleEndianIEEEDouble bitVal; 39 | real_T fltVal; 40 | } tmpVal; 41 | 42 | tmpVal.bitVal.words.wordH = 0xFFF80000U; 43 | tmpVal.bitVal.words.wordL = 0x00000000U; 44 | nan = tmpVal.fltVal; 45 | break; 46 | } 47 | 48 | case BigEndian: 49 | { 50 | union { 51 | BigEndianIEEEDouble bitVal; 52 | real_T fltVal; 53 | } tmpVal; 54 | 55 | tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; 56 | tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; 57 | nan = tmpVal.fltVal; 58 | break; 59 | } 60 | } 61 | } 62 | 63 | return nan; 64 | } 65 | 66 | /* Function: rtGetNaNF ================================================== 67 | * Abstract: 68 | * Initialize rtNaNF needed by the generated code. 69 | * NaN is initialized as non-signaling. Assumes IEEE. 70 | */ 71 | real32_T rtGetNaNF(void) 72 | { 73 | IEEESingle nanF = { { 0 } }; 74 | uint16_T one = 1U; 75 | enum { 76 | LittleEndian, 77 | BigEndian 78 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 79 | switch (machByteOrder) { 80 | case LittleEndian: 81 | { 82 | nanF.wordL.wordLuint = 0xFFC00000U; 83 | break; 84 | } 85 | 86 | case BigEndian: 87 | { 88 | nanF.wordL.wordLuint = 0x7FFFFFFFU; 89 | break; 90 | } 91 | } 92 | 93 | return nanF.wordL.wordLreal; 94 | } 95 | 96 | /* End of code generation (rtGetNaN.c) */ 97 | -------------------------------------------------------------------------------- /include/smp/components/extenders/MotionPrimitiveGeneratorCombiner.m: -------------------------------------------------------------------------------- 1 | %% MOTION PRIMITIVE GENERATOR 2 | 3 | % THIS SCRIPT GENERATES AND DISPLAYS MOTION PRIMITIVE BASE ON A UNICYCLE 4 | % KINEMATIC MODEL. 5 | % created by lp@srl 26.11.2013 6 | clear all 7 | % close all 8 | clc 9 | 10 | 11 | % Define the Velocities' ranges 12 | % WI is the angular velocity range 13 | 14 | 15 | % % %%% First set of motion primitives 16 | Wmin=-3; 17 | Wmax=3; 18 | Wstep=0.6; 19 | wI=Wmin:Wstep:Wmax; 20 | 21 | % VI is the translational velocity range 22 | Vmin=0.8; 23 | Vmax=2; 24 | Vstep=0.2; 25 | vI=Vmin:Vstep:Vmax; 26 | 27 | 28 | %% Second set of motion primitives 29 | % Wmin=-1.8; 30 | % Wmax=1.8; 31 | % Wstep=0.3; 32 | % wI=Wmin:Wstep:Wmax; 33 | % 34 | % % VI is the translational velocity range 35 | % Vmin=0.5; 36 | % Vmax=3.0; 37 | % Vstep=0.5; 38 | % vI=Vmin:Vstep:Vmax; 39 | 40 | 41 | % % % %%% Third set of motion primitives 42 | % Wmin=-4.8; 43 | % Wmax=4.8; 44 | % Wstep=0.6; 45 | % wI=Wmin:Wstep:Wmax; 46 | % 47 | % % VI is the translational velocity range 48 | % Vmin=0.5; 49 | % Vmax=4.0; 50 | % Vstep=0.5; 51 | % vI=Vmin:Vstep:Vmax; 52 | 53 | %%% 4th set of motion primitives 54 | 55 | % vI=[0.8 2]; 56 | % wI=[-2.6 -1.3 0 1.3 2.6] 57 | 58 | 59 | 60 | k=1; 61 | for i=1:length(vI) 62 | for j=1:length(wI) 63 | 64 | vM(k)=vI(i); 65 | wM(k)=wI(j); 66 | k=k+1; 67 | end 68 | end 69 | 70 | 71 | % define the motion primitive array 72 | motionPrimitiveCommandArray=[vM' wM']; 73 | q0=[0;0;0]; 74 | % Make sure that the time of integration is small!! 75 | ti=0; 76 | tf=0.3; 77 | dt=0.01; 78 | tSpan=ti:dt:tf; 79 | Qexport=[]; 80 | Nexport=[]; 81 | Vexport=[]; 82 | Wexport=[]; 83 | [nr nc]=size(motionPrimitiveCommandArray); 84 | figure(20),hold on 85 | for i=1:nr 86 | v = motionPrimitiveCommandArray(i,1); 87 | omega = motionPrimitiveCommandArray(i,2); 88 | [T Q] = ode45(@(t,q) unicycleKinematics( t, q, v ,omega ),tSpan,q0); % solve ODE 89 | 90 | figure(20), plot(Q(:,1),Q(:,2),'b','LineWidth',1) 91 | [n1r nc]=size(Q); 92 | Nexport=[Nexport;n1r]; 93 | Qexport=[Qexport; Q(:,:)]; 94 | Vexport=[Vexport;v]; 95 | Wexport=[Wexport;omega]; 96 | 97 | 98 | end 99 | axis equal,box on, 100 | axisx=xlabel('x [m]') 101 | set(axisx,'FontSize',16); 102 | axisy=ylabel('y [m]') 103 | set(axisy,'FontSize',16); 104 | set(findall(figure(20),'type','text'),'fontSize',18,'fontWeight','light') 105 | % set(gca,'XTick',[],'YTick',[]),box on 106 | 107 | 108 | -------------------------------------------------------------------------------- /include/smp/components/samplers/gauss.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/gauss.h 2 | \brief The uniform sampler for Circle Region 3 | 4 | The sampler provides random samples of states that are uniformly distributed in 5 | a circle bounded region. 6 | */ 7 | 8 | #ifndef _SMP_SAMPLER_GAUSS_H_ 9 | #define _SMP_SAMPLER_GAUSS_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace smp { 17 | 18 | //! Implements the sampler components that relies on uniform sampling. 19 | /*! 20 | A sampler component that implements uniform sampling. 21 | 22 | \ingroup samplers 23 | */ 24 | template < class typeparams, int NUM_DIMENSIONS > 25 | class sampler_gauss : public sampler_base< typeparams > { 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::vertex_data vertex_data_t; 29 | typedef typename typeparams::edge_data edge_data_t; 30 | 31 | typedef vertex vertex_t; 32 | typedef edge edge_t; 33 | 34 | typedef regionc region_t; 35 | 36 | region_t support; 37 | 38 | public: 39 | sampler_gauss (); 40 | ~sampler_gauss (); 41 | 42 | 43 | int sm_update_insert_vertex (vertex_t *vertex_in); 44 | 45 | 46 | int sm_update_insert_edge (edge_t *edge_in); 47 | 48 | 49 | int sm_update_delete_vertex (vertex_t *vertex_in); 50 | 51 | 52 | int sm_update_delete_edge (edge_t *edge_in); 53 | 54 | 55 | int sample (state_t **state_sample_out); 56 | 57 | /** 58 | * \brief Sets the dimensions and position of the rectangular bounding box of 59 | * the support. 60 | * 61 | * Uniform distribution only makes sense in a bounded support, which can be set 62 | * using this function. This sampler function only draws samples from a rectangular 63 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 64 | * parameter to the uniform sampler class. If the support variable is not set, i.e., 65 | * this function is never called, then the support is initialized to the unit cube 66 | * centered at the origin by default. 67 | * 68 | * @param support_in New support for the uniform sampling distribution. 69 | * 70 | * @returns Returns 1 for success, a non-positive number for failure. 71 | */ 72 | int set_support (const region_t support_in); 73 | 74 | 75 | }; 76 | 77 | 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /include/smp/components/samplers/halton.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/halton.h 2 | \brief The halton deterministic sampler 3 | 4 | The sampler provides random samples of states that are uniformly distributed in 5 | a bounded region. 6 | */ 7 | 8 | #ifndef _SMP_SAMPLER_HALTON_H_ 9 | #define _SMP_SAMPLER_HALTON_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace smp { 16 | 17 | //! Implements the sampler components that relies on halton sampling. 18 | /*! 19 | A sampler component that implements halton sampling. 20 | 21 | \ingroup samplers 22 | */ 23 | template < class typeparams, int NUM_DIMENSIONS > 24 | class sampler_halton : public sampler_base< typeparams > { 25 | 26 | typedef typename typeparams::state state_t; 27 | typedef typename typeparams::vertex_data vertex_data_t; 28 | typedef typename typeparams::edge_data edge_data_t; 29 | 30 | typedef vertex vertex_t; 31 | typedef edge edge_t; 32 | 33 | typedef region region_t; 34 | 35 | region_t support; 36 | 37 | public: 38 | sampler_halton (); 39 | ~sampler_halton (); 40 | 41 | 42 | int sm_update_insert_vertex (vertex_t *vertex_in); 43 | 44 | 45 | int sm_update_insert_edge (edge_t *edge_in); 46 | 47 | 48 | int sm_update_delete_vertex (vertex_t *vertex_in); 49 | 50 | 51 | int sm_update_delete_edge (edge_t *edge_in); 52 | 53 | 54 | int sample (state_t **state_sample_out); 55 | 56 | /** 57 | * \brief Sets the dimensions and position of the rectangular bounding box of 58 | * the support. 59 | * 60 | * Halton distribution only makes sense in a bounded support, which can be set 61 | * using this function. This sampler function only draws samples from a rectangular 62 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 63 | * parameter to the Halton sampler class. If the support variable is not set, i.e., 64 | * this function is never called, then the support is initialized to the unit cube 65 | * centered at the origin by default. 66 | * 67 | * @param support_in New support for the Halton sampling distribution. 68 | * 69 | * @returns Returns 1 for success, a non-positive number for failure. 70 | */ 71 | int set_support (const region_t support_in); 72 | 73 | 74 | }; 75 | 76 | 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /include/smp/components/samplers/uniformc.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/uniformc.h 2 | \brief The uniform sampler for Circle Region 3 | 4 | The sampler provides random samples of states that are uniformly distributed in 5 | a circle bounded region. 6 | */ 7 | 8 | #ifndef _SMP_SAMPLER_UNIFORMC_H_ 9 | #define _SMP_SAMPLER_UNIFORMC_H_ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace smp { 17 | 18 | //! Implements the sampler components that relies on uniform sampling. 19 | /*! 20 | A sampler component that implements uniform sampling. 21 | 22 | \ingroup samplers 23 | */ 24 | template < class typeparams, int NUM_DIMENSIONS > 25 | class sampler_uniformc : public sampler_base< typeparams > { 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::vertex_data vertex_data_t; 29 | typedef typename typeparams::edge_data edge_data_t; 30 | 31 | typedef vertex vertex_t; 32 | typedef edge edge_t; 33 | 34 | typedef regionc region_t; 35 | 36 | region_t support; 37 | 38 | public: 39 | sampler_uniformc (); 40 | ~sampler_uniformc (); 41 | 42 | 43 | int sm_update_insert_vertex (vertex_t *vertex_in); 44 | 45 | 46 | int sm_update_insert_edge (edge_t *edge_in); 47 | 48 | 49 | int sm_update_delete_vertex (vertex_t *vertex_in); 50 | 51 | 52 | int sm_update_delete_edge (edge_t *edge_in); 53 | 54 | 55 | int sample (state_t **state_sample_out); 56 | 57 | /** 58 | * \brief Sets the dimensions and position of the rectangular bounding box of 59 | * the support. 60 | * 61 | * Uniform distribution only makes sense in a bounded support, which can be set 62 | * using this function. This sampler function only draws samples from a rectangular 63 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 64 | * parameter to the uniform sampler class. If the support variable is not set, i.e., 65 | * this function is never called, then the support is initialized to the unit cube 66 | * centered at the origin by default. 67 | * 68 | * @param support_in New support for the uniform sampling distribution. 69 | * 70 | * @returns Returns 1 for success, a non-positive number for failure. 71 | */ 72 | int set_support (const region_t support_in); 73 | 74 | 75 | }; 76 | 77 | 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /include/smp/components/samplers/halton.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_HALTON_HPP_ 2 | #define _SMP_SAMPLER_HALTON_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | template< class typeparams, int NUM_DIMENSIONS > 14 | int smp::sampler_halton 15 | ::sm_update_insert_vertex (vertex_t *vertex_in) { 16 | 17 | return 1; 18 | } 19 | 20 | 21 | template< class typeparams, int NUM_DIMENSIONS > 22 | int smp::sampler_halton 23 | ::sm_update_insert_edge (edge_t *edge_in) { 24 | 25 | return 1; 26 | } 27 | 28 | 29 | template< class typeparams, int NUM_DIMENSIONS > 30 | int smp::sampler_halton 31 | ::sm_update_delete_vertex (vertex_t *vertex_in) { 32 | 33 | return 1; 34 | } 35 | 36 | 37 | template< class typeparams, int NUM_DIMENSIONS > 38 | int smp::sampler_halton 39 | ::sm_update_delete_edge (edge_t *edge_in) { 40 | 41 | return 1; 42 | } 43 | 44 | 45 | template< class typeparams, int NUM_DIMENSIONS > 46 | smp::sampler_halton 47 | ::sampler_halton () { 48 | 49 | // Initialize the sampling distribution support. 50 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 51 | support.center[i] = 0.0; 52 | support.size[i] = 1.0; 53 | } 54 | 55 | // Initialize the halton sequence dimension 56 | halton_dim_num_set (NUM_DIMENSIONS); 57 | 58 | } 59 | 60 | 61 | template< class typeparams, int NUM_DIMENSIONS > 62 | smp::sampler_halton 63 | ::~sampler_halton () { 64 | 65 | 66 | } 67 | 68 | 69 | template< class typeparams, int NUM_DIMENSIONS > 70 | int smp::sampler_halton 71 | ::sample (state_t **state_sample_out) { 72 | 73 | if (NUM_DIMENSIONS <= 0) 74 | return 0; 75 | 76 | state_t *state_new = new state_t; 77 | 78 | double halton_sample[NUM_DIMENSIONS]; 79 | 80 | halton (halton_sample); 81 | 82 | 83 | // Generate an independent random variable for each axis. 84 | for (int i = 0; i < NUM_DIMENSIONS; i++) 85 | (*state_new)[i] = support.size[i] * halton_sample[i] - support.size[i]/2.0 + support.center[i]; 86 | 87 | *state_sample_out = state_new; 88 | 89 | return 1; 90 | } 91 | 92 | 93 | template< class typeparams, int NUM_DIMENSIONS > 94 | int smp::sampler_halton 95 | ::set_support (region_t support_in) { 96 | 97 | support = support_in; 98 | 99 | return 1; 100 | } 101 | 102 | 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/mapminmax_apply47_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 58
 3 | 59% ===== MODULE FUNCTIONS ========
 4 | 60
 5 | 61% Map Minimum and Maximum Input Processing Function
 6 | 62function y = mapminmax_apply(x,settings_gain,settings_xoffset,settings_ymin)
 7 | 63  y = bsxfun(@minus,x,settings_xoffset);
 8 | 64  y = bsxfun(@times,y,settings_gain);
 9 | 65  y = bsxfun(@plus,y,settings_ymin);
10 | 66end
11 | 67
12 | 68% Sigmoid Symmetric Transfer Function
13 |
14 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/mapminmax_apply47_mcode.html: -------------------------------------------------------------------------------- 1 | 
 2 | 58
 3 | 59% ===== MODULE FUNCTIONS ========
 4 | 60
 5 | 61% Map Minimum and Maximum Input Processing Function
 6 | 62function y = mapminmax_apply(x,settings_gain,settings_xoffset,settings_ymin)
 7 | 63  y = bsxfun(@minus,x,settings_xoffset);
 8 | 64  y = bsxfun(@times,y,settings_gain);
 9 | 65  y = bsxfun(@plus,y,settings_ymin);
10 | 66end
11 | 67
12 | 68% Sigmoid Symmetric Transfer Function
13 |
14 | -------------------------------------------------------------------------------- /include/smp/components/samplers/theta_star_gaussian.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/theta_star_gaussian.h 2 | \brief Sampling Unit which generates samples along a theta* path. 3 | 4 | The sampler provides random samples of states that are normal distributed in 5 | */ 6 | theta_star_gaussian 7 | #ifndef _SMP_THETA_STAR_GAUSSIAN_H_ 8 | #define _SMP_THETA_STAR_GAUSSIAN_H_ 9 | 10 | #include 11 | 12 | 13 | #include 14 | 15 | namespace smp { 16 | 17 | //! Implements the sampler components that relies on uniform sampling. 18 | /*! 19 | A sampler component that implements gaussian sampling along a computed discrete path. 20 | 21 | \ingroup samplers 22 | */ 23 | template < class typeparams, int NUM_DIMENSIONS > 24 | class theta_star_gaussian : public sampler_base< typeparams > { 25 | 26 | typedef typename typeparams::state state_t; 27 | typedef typename typeparams::vertex_data vertex_data_t; 28 | typedef typename typeparams::edge_data edge_data_t; 29 | 30 | typedef vertex vertex_t; 31 | typedef edge edge_t; 32 | 33 | typedef region region_t; 34 | 35 | region_t support; 36 | 37 | public: 38 | theta_star_gaussian (); 39 | ~theta_star_gaussian (); 40 | 41 | 42 | int sm_update_insert_vertex (vertex_t *vertex_in); 43 | 44 | 45 | int sm_update_insert_edge (edge_t *edge_in); 46 | 47 | 48 | int sm_update_delete_vertex (vertex_t *vertex_in); 49 | 50 | 51 | int sm_update_delete_edge (edge_t *edge_in); 52 | 53 | 54 | int sample (state_t **state_sample_out); 55 | 56 | /** 57 | * \brief Sets the dimensions and position of the rectangular bounding box of 58 | * the support. 59 | * 60 | * Uniform distribution only makes sense in a bounded support, which can be set 61 | * using this function. This sampler function only draws samples from a rectangular 62 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 63 | * parameter to the uniform sampler class. If the support variable is not set, i.e., 64 | * this function is never called, then the support is initialized to the unit cube 65 | * centered at the origin by default. 66 | * 67 | * @param support_in New support for the uniform sampling distribution. 68 | * 69 | * @returns Returns 1 for success, a non-positive number for failure. 70 | */ 71 | int set_support (const region_t support_in); 72 | 73 | 74 | 75 | 76 | }; 77 | 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /include/smp/components/samplers/eigenmultivariatenormal.cpp: -------------------------------------------------------------------------------- 1 | #ifndef __EIGENMULTIVARIATENORMAL_HPP 2 | #define __EIGENMULTIVARIATENORMAL_HPP 3 | 4 | #include "Eigen/Dense" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | /** 13 | * find the eigen-decomposition of the covariance matrix. 14 | * create a vector of normal samples scaled by the eigenvalues. 15 | * rotate the vector by the eigenvectors. 16 | * add the mean. 17 | */ 18 | 19 | 20 | template 21 | 22 | 23 | 24 | class EigenMultivariateNormal 25 | { 26 | 27 | 28 | public: 29 | boost::mt19937 rng ; // The uniform pseudo-random algorithm 30 | boost::normal_distribution<_Scalar> norm; // The gaussian combinator 31 | boost::variate_generator > 32 | randN; // The 0-mean unit-variance normal generator 33 | 34 | Eigen::Matrix<_Scalar,_size,_size> rot; 35 | Eigen::Matrix<_Scalar,_size,1> scl; 36 | Eigen::Matrix<_Scalar,_size,1> mean; 37 | EigenMultivariateNormal(const Eigen::Matrix<_Scalar,_size,1>& meanVec,const Eigen::Matrix<_Scalar,_size,_size>& covarMat): randN(rng,norm) 38 | { 39 | 40 | rng.seed(static_cast< double>(std::time(0))); 41 | randN.engine().seed(static_cast< double>(std::time(0))); 42 | randN.distribution().reset(); 43 | //std::cout<(std::time(0))<(std::time(0))*i); 52 | randN.engine().seed(static_cast< double>(std::time(0))*i); 53 | randN.distribution().reset(); 54 | 55 | } 56 | 57 | 58 | void setCovar(const Eigen::Matrix<_Scalar,_size,_size>& covarMat) 59 | { 60 | Eigen::SelfAdjointEigenSolver > 61 | eigenSolver(covarMat); 62 | rot = eigenSolver.eigenvectors(); 63 | scl = eigenSolver.eigenvalues(); 64 | for (int ii=0;ii<_size;++ii) { 65 | scl(ii,0) = sqrt(scl(ii,0)); 66 | 67 | } 68 | } 69 | 70 | void setMean(const Eigen::Matrix<_Scalar,_size,1>& meanVec) 71 | { 72 | mean = meanVec; 73 | } 74 | 75 | void nextSample(Eigen::Matrix<_Scalar,_size,1>& sampleVec) 76 | { 77 | for (int ii=0;ii<_size;++ii) { 78 | sampleVec(ii,0) = randN()*scl(ii,0); 79 | } 80 | sampleVec = rot*sampleVec + mean; 81 | } 82 | 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/costotogo_buildlog1.html: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 | 5 | 23 |
24 | Code Generation Report For 'costotogo' 25 | 26 | 27 |
MATLAB Coder Build Log

28 |

29 |
30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 |
Build Parameters
Build directory/home/luigi/code/palmieri/learningcost2go/codegen/exe/costotogo
Make wrapper/usr/local/MATLAB/R2013b/bin/glnxa64/gmake -f costotogo_rtw.mk all
Make command/usr/local/MATLAB/R2013b/bin/glnxa64/gmake -f costotogo_rtw.mk all
40 |
41 | 42 | 43 |
Build Log
44 | 
45 |     1   Compilation suppressed: generating code only.
46 |
47 |
48 |
49 | 50 | 51 | -------------------------------------------------------------------------------- /config/global_costmap_params_simulated.yaml: -------------------------------------------------------------------------------- 1 | # do not use rolling window version of costmap, as global costmap is static 2 | rolling_window: false 3 | # global map will be publish at modrate frequency of 10 4 | publish_frequency: 10.0 5 | # resolution of the map in meters/cell 6 | resolution: 0.20 7 | # global costmap will operate in the map frame, transform between this and robot-base frame must be available 8 | global_frame: map 9 | 10 | static_map: true 11 | 12 | # name of the frame at base-link of the spencer robot 13 | robot_base_frame: base_link 14 | # specifications for the footprint of the robot as polygon (in meters) 15 | # footprint: [ [0.756, 0.455], [0.756, -0.455], [-0.791, -0.455], [-0.791, 0.455] ] # real footprint 16 | footprint: [[0.796, 0.495], [0.796, -0.495], [-0.831, -0.495], [-0.831, 0.495] ] # larger footprint 17 | #footprint: [[0.39, 0.27], [0.39, -0.27], [0.24, -0.41], [-0.24, -0.41], [-0.39, -0.27], [-0.39, 0.27], [-0.24, 0.41], [0.24, 0.41]] 18 | origin_x: 0.0 19 | origin_y: 0.0 20 | 21 | # for global plannign we will use static-map, obstacles, inflation and hanp layers 22 | plugins: 23 | - {name: static_map, type: "costmap_2d::StaticLayer"} 24 | - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 25 | - {name: inflater, type: "costmap_2d::InflationLayer"} 26 | # - {name: social_compliance, type: "social_compliance_layer::SocialComplianceLayer"} 27 | 28 | 29 | obstacles: 30 | observation_sources: /pedsim/obstacle_cloud_global /pedsim/people_cloud_global 31 | /pedsim/obstacle_cloud_global: {data_type: PointCloud, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 32 | /pedsim/people_cloud_global: {data_type: PointCloud, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 33 | 34 | inflater: 35 | observation_sources: /pedsim/obstacle_cloud_global /pedsim/people_cloud_global 36 | /pedsim/obstacle_cloud_global: {data_type: PointCloud, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 37 | /pedsim/people_cloud_global: {data_type: PointCloud, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 38 | 39 | # social_compliance: 40 | # observation_sources: /spencer/perception/tracked_persons_filtered_by_distance /spencer/perception/tracked_groups 41 | # /spencer/perception/tracked_persons_filtered_by_distance: {data_type: TrackedPersons, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 42 | # /spencer/perception/tracked_groups: {data_type: TrackedGroups, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 43 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/costotogo_buildlog1.html: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 | 5 | 23 |
24 | Code Generation Report For 'costotogo' 25 | 26 | 27 |
MATLAB Coder Build Log

28 |

29 |
30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 |
Build Parameters
Build directory/home/luigi/code/palmieri/learningcost2go/codegen/exe/costotogo
Make wrapper/usr/local/MATLAB/R2013b/bin/glnxa64/gmake -f costotogo_rtw.mk all
Make command/usr/local/MATLAB/R2013b/bin/glnxa64/gmake -f costotogo_rtw.mk all
40 |
41 | 42 | 43 |
Build Log
44 | 
45 |     1   Compilation suppressed: generating code only.
46 |
47 |
48 |
49 | 50 | 51 | -------------------------------------------------------------------------------- /include/smp/components/samplers/uniformc.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_UNIFORMC_HPP_ 2 | #define _SMP_SAMPLER_UNIFORMC_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | #include "eigenmultivariatenormal.cpp" 14 | 15 | 16 | template< class typeparams, int NUM_DIMENSIONS > 17 | int smp::sampler_uniformc 18 | ::sm_update_insert_vertex (vertex_t *vertex_in) { 19 | 20 | return 1; 21 | } 22 | 23 | 24 | template< class typeparams, int NUM_DIMENSIONS > 25 | int smp::sampler_uniformc 26 | ::sm_update_insert_edge (edge_t *edge_in) { 27 | 28 | return 1; 29 | } 30 | 31 | 32 | template< class typeparams, int NUM_DIMENSIONS > 33 | int smp::sampler_uniformc 34 | ::sm_update_delete_vertex (vertex_t *vertex_in) { 35 | 36 | return 1; 37 | } 38 | 39 | 40 | template< class typeparams, int NUM_DIMENSIONS > 41 | int smp::sampler_uniformc 42 | ::sm_update_delete_edge (edge_t *edge_in) { 43 | 44 | return 1; 45 | } 46 | 47 | 48 | template< class typeparams, int NUM_DIMENSIONS > 49 | smp::sampler_uniformc 50 | ::sampler_uniformc() { 51 | 52 | // Initialize the sampling distribution support. 53 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 54 | support.center[i] = 0.0; 55 | support.size[i] = 1.0; 56 | } 57 | } 58 | 59 | 60 | template< class typeparams, int NUM_DIMENSIONS > 61 | smp::sampler_uniformc 62 | ::~sampler_uniformc () { 63 | 64 | 65 | } 66 | 67 | 68 | 69 | /// Sample function for a Circle Region 70 | template< class typeparams, int NUM_DIMENSIONS > 71 | int smp::sampler_uniformc 72 | ::sample (state_t **state_sample_out) { 73 | 74 | double radius,angle; 75 | if (NUM_DIMENSIONS <= 0) 76 | return 0; 77 | 78 | state_t *state_new = new state_t; 79 | radius=sqrt((support.radius/2 * rand()/(RAND_MAX + 1.0))); 80 | angle=2*M_PI*rand()/(RAND_MAX + 1.0); 81 | (*state_new)[0]=radius*cos(angle)+ support.center[0]; 82 | (*state_new)[1]=radius*sin(angle)+ support.center[1]; 83 | // (*state_new)[2]=support.ang_range*rand()/(RAND_MAX + 1.0)-support.ang_range/2+support.center[2]; 84 | (*state_new)[NUM_DIMENSIONS-1]=support.center[NUM_DIMENSIONS-1]; 85 | *state_sample_out = state_new; 86 | 87 | return 1; 88 | } 89 | 90 | template< class typeparams, int NUM_DIMENSIONS > 91 | int smp::sampler_uniformc 92 | ::set_support (region_t support_in) { 93 | 94 | support = support_in; 95 | 96 | return 1; 97 | } 98 | 99 | 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /include/smp/components/model_checkers/reachability.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_MODEL_CHECKER_REACHABILITY_HPP_ 2 | #define _SMP_MODEL_CHECKER_REACHABILITY_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | template < class typeparams, int NUM_DIMENSIONS > 11 | smp::model_checker_reachability 12 | ::model_checker_reachability () { 13 | 14 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 15 | region_goal.center[i] = 0.0; 16 | region_goal.size[i] = 0.0; 17 | } 18 | 19 | } 20 | 21 | 22 | template < class typeparams, int NUM_DIMENSIONS > 23 | smp::model_checker_reachability 24 | ::~model_checker_reachability () { 25 | 26 | 27 | } 28 | 29 | 30 | template < class typeparams, int NUM_DIMENSIONS > 31 | smp::model_checker_reachability 32 | ::model_checker_reachability (const region_t ®ion_in) { 33 | 34 | region_goal = region_in; 35 | } 36 | 37 | 38 | template < class typeparams, int NUM_DIMENSIONS > 39 | int smp::model_checker_reachability 40 | ::set_goal_region (const region_t ®ion_in) { 41 | 42 | region_goal = region_in; 43 | 44 | return 1; 45 | } 46 | 47 | 48 | template < class typeparams, int NUM_DIMENSIONS > 49 | int smp::model_checker_reachability 50 | ::mc_update_insert_vertex (vertex_t *vertex_in) { 51 | 52 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 53 | 54 | //double state_var_curr = (*vertex_in->state)[i]; 55 | 56 | if (fabs ((*vertex_in->state)[i] - region_goal.center[i]) > region_goal.size[i]) { 57 | vertex_in->data.reaches_goal = 0; 58 | return 1; 59 | } 60 | } 61 | 62 | vertex_in->data.reaches_goal = 1; 63 | 64 | return 1; 65 | } 66 | 67 | 68 | template < class typeparams, int NUM_DIMENSIONS > 69 | int smp::model_checker_reachability 70 | ::mc_update_insert_edge (edge_t *edge_in) { 71 | 72 | return 1; 73 | } 74 | 75 | 76 | template < class typeparams, int NUM_DIMENSIONS > 77 | int smp::model_checker_reachability 78 | ::mc_update_delete_vertex (vertex_t *vertex_in) { 79 | 80 | return 1; 81 | } 82 | 83 | 84 | template < class typeparams, int NUM_DIMENSIONS > 85 | int smp::model_checker_reachability 86 | ::mc_update_delete_edge (edge_t *edge_in) { 87 | 88 | return 1; 89 | } 90 | 91 | 92 | template < class typeparams, int NUM_DIMENSIONS > 93 | int smp::model_checker_reachability 94 | ::get_solution (trajectory_t &trajectory_out) { 95 | 96 | trajectory_out.clear (); 97 | 98 | return 1; 99 | } 100 | 101 | 102 | #endif 103 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/costotogo_types_h.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_types.h 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_types.h

11 | 
12 |     1   /*
13 |     2    * costotogo_types.h
14 |     3    *
15 |     4    * Code generation for function 'costotogo'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   #ifndef __COSTOTOGO_TYPES_H__
22 |    11   #define __COSTOTOGO_TYPES_H__
23 |    12   
24 |    13   /* Include files */
25 |    14   #include "rtwtypes.h"
26 |    15   
27 |    16   #endif
28 |    17   /* End of code generation (costotogo_types.h) */
29 |    18
30 |
31 |

32 | 33 | -------------------------------------------------------------------------------- /config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | # do not use rolling window version of costmap, as global costmap is static 2 | rolling_window: false 3 | # global map will be publish at modrate frequency of 10 4 | publish_frequency: 10.0 5 | # resolution of the map in meters/cell 6 | resolution: 0.20 7 | # global costmap will operate in the map frame, transform between this and robot-base frame must be available 8 | global_frame: occ_map 9 | 10 | static_map: true 11 | 12 | # name of the frame at base-link of the spencer robot 13 | robot_base_frame: base_link 14 | # specifications for the footprint of the robot as polygon (in meters) 15 | # footprint: [ [0.756, 0.455], [0.756, -0.455], [-0.791, -0.455], [-0.791, 0.455] ] # real footprint 16 | footprint: [[0.796, 0.495], [0.796, -0.495], [-0.831, -0.495], [-0.831, 0.495] ] # larger footprint 17 | #footprint: [[0.39, 0.27], [0.39, -0.27], [0.24, -0.41], [-0.24, -0.41], [-0.39, -0.27], [-0.39, 0.27], [-0.24, 0.41], [0.24, 0.41]] 18 | origin_x: 0.0 19 | origin_y: 0.0 20 | 21 | # for global plannign we will use static-map, obstacles, inflation and hanp layers 22 | plugins: 23 | - {name: static_map, type: "costmap_2d::StaticLayer"} 24 | # - {name: obstacles, type: "costmap_2d::ObstacleLayer"} 25 | - {name: inflater, type: "costmap_2d::InflationLayer"} 26 | # - {name: social_compliance, type: "social_compliance_layer::SocialComplianceLayer"} 27 | 28 | # NOTE!! that including obstacles it may happen that in narrow corridors echos from the laser scanner may appear as obstacles! 29 | # Not needed anyway cause using the static map layer 30 | #obstacles: 31 | # observation_sources: /spencer/sensors/laser_rear/echo0 /spencer/sensors/laser_front/echo0 32 | # /spencer/sensors/laser_rear/echo0: {data_type: LaserScan, sensor_frame: /laser_rear_link, marking: true, clearing: true} 33 | # /spencer/sensors/laser_front/echo0: {data_type: LaserScan, sensor_frame: /laser_front_link, marking: true, clearing: true} 34 | 35 | # inflater: 36 | # observation_sources: /spencer/sensors/laser_rear/echo0 /spencer/sensors/laser_front/echo0 37 | # /spencer/sensors/laser_rear/echo0: {data_type: LaserScan, sensor_frame: /laser_rear_link, marking: true, clearing: true, map_type: costmap} 38 | # /spencer/sensors/laser_front/echo0: {data_type: LaserScan, sensor_frame: /laser_front_link, marking: true, clearing: true, map_type: costmap} 39 | # inflation_radius: 0.1 40 | 41 | # social_compliance: 42 | # observation_sources: /spencer/perception/tracked_persons_filtered_by_distance /spencer/perception/tracked_groups 43 | # /spencer/perception/tracked_persons_filtered_by_distance: {data_type: TrackedPersons, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 44 | # /spencer/perception/tracked_groups: {data_type: TrackedGroups, sensor_frame: odom, marking: true, clearing: true, obstacle_range: 30, raytrace_range: 30, map_type: costmap} 45 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/costotogo_types_h.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_types.h 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_types.h

11 | 
12 |     1   /*
13 |     2    * costotogo_types.h
14 |     3    *
15 |     4    * Code generation for function 'costotogo'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   #ifndef __COSTOTOGO_TYPES_H__
22 |    11   #define __COSTOTOGO_TYPES_H__
23 |    12   
24 |    13   /* Include files */
25 |    14   #include "rtwtypes.h"
26 |    15   
27 |    16   #endif
28 |    17   /* End of code generation (costotogo_types.h) */
29 |    18
30 |
31 |

32 | 33 | -------------------------------------------------------------------------------- /include/smp/components/samplers/uniform.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/uniform.h 2 | \brief The uniform sampler 3 | 4 | The sampler provides random samples of states that are uniformly distributed in 5 | a bounded region. 6 | */ 7 | 8 | #ifndef _SMP_SAMPLER_UNIFORM_H_ 9 | #define _SMP_SAMPLER_UNIFORM_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace smp { 16 | 17 | //! Implements the sampler components that relies on uniform sampling. 18 | /*! 19 | A sampler component that implements uniform sampling. 20 | 21 | \ingroup samplers 22 | */ 23 | template < class typeparams, int NUM_DIMENSIONS > 24 | class sampler_uniform : public sampler_base< typeparams > { 25 | 26 | typedef typename typeparams::state state_t; 27 | typedef typename typeparams::vertex_data vertex_data_t; 28 | typedef typename typeparams::edge_data edge_data_t; 29 | 30 | typedef vertex vertex_t; 31 | typedef edge edge_t; 32 | 33 | // typedef region region_t; 34 | typedef region region_t; 35 | 36 | region_t support; 37 | 38 | public: 39 | sampler_uniform (); 40 | ~sampler_uniform (); 41 | 42 | 43 | int sm_update_insert_vertex (vertex_t *vertex_in); 44 | 45 | 46 | int sm_update_insert_edge (edge_t *edge_in); 47 | 48 | 49 | int sm_update_delete_vertex (vertex_t *vertex_in); 50 | 51 | 52 | int sm_update_delete_edge (edge_t *edge_in); 53 | 54 | 55 | int sample (state_t **state_sample_out); 56 | 57 | /** 58 | * \brief Sets the dimensions and position of the rectangular bounding box of 59 | * the support. 60 | * 61 | * Uniform distribution only makes sense in a bounded support, which can be set 62 | * using this function. This sampler function only draws samples from a rectangular 63 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 64 | * parameter to the uniform sampler class. If the support variable is not set, i.e., 65 | * this function is never called, then the support is initialized to the unit cube 66 | * centered at the origin by default. 67 | * 68 | * @param support_in New support for the uniform sampling distribution. 69 | * 70 | * @returns Returns 1 for success, a non-positive number for failure. 71 | */ 72 | int set_support (const region_t support_in); 73 | 74 | 75 | int set_goal( region_t g); 76 | 77 | int set_goal_biasing(int option); 78 | 79 | int set_goal_biasing_ths(double p); 80 | 81 | int GOAL_BIASING; 82 | region_t goal; 83 | double P_THS; 84 | 85 | 86 | }; 87 | 88 | 89 | } 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /include/smp/utils/branch_and_bound_euclidean.h: -------------------------------------------------------------------------------- 1 | /*! \file branch_and_bound_euclidean.h 2 | \brief Branch and bound with the Euclidean distance heuristic 3 | */ 4 | 5 | #ifndef _SMP_BRANCH_AND_BOUND_EUCLIDEAN_H_ 6 | #define _SMP_BRANCH_AND_BOUND_EUCLIDEAN_H_ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | 14 | namespace smp{ 15 | 16 | //! Branch and bound with the Euclidean distance admissible heuristic (not considering obstacles). 17 | /*! 18 | Currently, the heuristic is not computing the exact distance. The current heuristic is not even 19 | admissible. It should be employed with care. 20 | 21 | \ingroup bnb 22 | */ 23 | template< class typeparams, int NUM_DIMENSIONS > 24 | class branch_and_bound_euclidean : public branch_and_bound_base { 25 | 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::input input_t; 29 | typedef typename typeparams::vertex_data vertex_data_t; 30 | typedef typename typeparams::edge_data edge_data_t; 31 | 32 | typedef region region_t; 33 | 34 | typedef vertex vertex_t; 35 | typedef edge edge_t; 36 | 37 | typedef planner planner_t; 38 | 39 | region_t region_goal; 40 | 41 | vertex_t *root_vertex; 42 | 43 | int add_children_to_list (list &list_vertices_in, vertex_t *vertex_in); 44 | 45 | public : 46 | 47 | branch_and_bound_euclidean (); 48 | ~branch_and_bound_euclidean (); 49 | 50 | 51 | int run_branch_and_bound (); 52 | 53 | /** 54 | * \brief Sets the goal region to which the Euclidean distance will be computed. 55 | * 56 | * The branch and bound with the Euclidean distance heuristic considers the Euclidean 57 | * distance neglegting the obstacles. It is assumed that the goal region is a box, which 58 | * can be set or modified using this function. 59 | * 60 | * @param region_goal_in New goal region. 61 | * 62 | * @returns Returns 1 for success, and a non-positive value to indicate error. 63 | */ 64 | int set_goal_region (region_t region_goal_in); 65 | 66 | 67 | /** 68 | * \brief Sets the root vertex of the planner. 69 | * 70 | * This function can be used to provide the heuristic with the knowledge of the root vertex. 71 | * Since the heuristic is not admissible, it has the danger of deleting the root vertex, which 72 | * can be avoided if a pointer to the root vertex is provided to the heuristic using 73 | * this function. 74 | * 75 | * @param root_vertex_in A pointer to the root vertex of the incremental planner. 76 | * 77 | * @returns Returns 1 for success, and a non-positive value to indicate error. 78 | */ 79 | int set_root_vertex (vertex_t *root_vertex_in); 80 | }; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/smp/components/samplers/gauss.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_GAUSS_HPP_ 2 | #define _SMP_SAMPLER_GAUSS_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | #include "eigenmultivariatenormal.cpp" 14 | 15 | 16 | template< class typeparams, int NUM_DIMENSIONS > 17 | int smp::sampler_gauss 18 | ::sm_update_insert_vertex (vertex_t *vertex_in) { 19 | 20 | return 1; 21 | } 22 | 23 | 24 | template< class typeparams, int NUM_DIMENSIONS > 25 | int smp::sampler_gauss 26 | ::sm_update_insert_edge (edge_t *edge_in) { 27 | 28 | return 1; 29 | } 30 | 31 | 32 | template< class typeparams, int NUM_DIMENSIONS > 33 | int smp::sampler_gauss 34 | ::sm_update_delete_vertex (vertex_t *vertex_in) { 35 | 36 | return 1; 37 | } 38 | 39 | 40 | template< class typeparams, int NUM_DIMENSIONS > 41 | int smp::sampler_gauss 42 | ::sm_update_delete_edge (edge_t *edge_in) { 43 | 44 | return 1; 45 | } 46 | 47 | 48 | template< class typeparams, int NUM_DIMENSIONS > 49 | smp::sampler_gauss 50 | ::sampler_gauss() { 51 | 52 | // Initialize the sampling distribution support. 53 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 54 | support.center[i] = 0.0; 55 | support.size[i] = 1.0; 56 | } 57 | } 58 | 59 | 60 | template< class typeparams, int NUM_DIMENSIONS > 61 | smp::sampler_gauss 62 | ::~sampler_gauss () { 63 | 64 | 65 | } 66 | 67 | 68 | 69 | /// Sample function for a Circle Region 70 | template< class typeparams, int NUM_DIMENSIONS > 71 | int smp::sampler_gauss 72 | ::sample (state_t **state_sample_out) { 73 | 74 | // Sampling by Using Multivariate Gaussians 75 | Eigen::Vector2d mean; 76 | Eigen::Matrix2d covar; 77 | Eigen::Vector2d ns; 78 | double sigmaxi,sigmayi; 79 | if (NUM_DIMENSIONS <= 0) 80 | return 0; 81 | 82 | state_t *state_new = new state_t; 83 | 84 | mean << support.center[0],support.center[1]; 85 | sigmaxi=1; 86 | sigmayi=1; 87 | Eigen::Matrix2d rot ; 88 | rot=Eigen::Rotation2Dd(0).matrix(); 89 | covar = rot*Eigen::DiagonalMatrix(sigmaxi,sigmayi)*rot.transpose(); 90 | EigenMultivariateNormal *normX = new EigenMultivariateNormal(mean,covar); 91 | (*normX).reseed(rand()+100*std::time(0)*rand()+std::time(0)); 92 | normX->nextSample(ns); 93 | // cout<<"DEBUG: sample --x,y--: -- "< 106 | int smp::sampler_gauss 107 | ::set_support (region_t support_in) { 108 | 109 | support = support_in; 110 | 111 | return 1; 112 | } 113 | 114 | 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /include/smp/components/samplers/opra.h: -------------------------------------------------------------------------------- 1 | /*! \file components/samplers/opra.h 2 | \brief The Opra sampler 3 | 4 | The sampler provides random samples of states that are distributed accordin to a Mixture of Gaussians along a path computed 5 | by an OPRA rule. 6 | */ 7 | 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | 15 | 16 | namespace smp { 17 | 18 | //! Implements the sampler components that relies on Opra sampling. 19 | /*! 20 | A sampler component that implements OPRA sampling. 21 | 22 | \ingroup samplers 23 | */ 24 | template < class typeparams, int NUM_DIMENSIONS > 25 | class sampler_opra : public sampler_base< typeparams > { 26 | 27 | typedef typename typeparams::state state_t; 28 | typedef typename typeparams::vertex_data vertex_data_t; 29 | typedef typename typeparams::edge_data edge_data_t; 30 | 31 | typedef vertex vertex_t; 32 | typedef edge edge_t; 33 | typedef trajectory trajectory_t; 34 | typedef typename typeparams::input input_t; 35 | 36 | typedef region region_t; 37 | 38 | region_t support; 39 | 40 | public: 41 | sampler_opra (); 42 | ~sampler_opra (); 43 | trajectory_t opra_trajectory; 44 | int indGen; 45 | 46 | int sm_update_insert_vertex (vertex_t *vertex_in); 47 | 48 | 49 | int sm_update_insert_edge (edge_t *edge_in); 50 | 51 | 52 | int sm_update_delete_vertex (vertex_t *vertex_in); 53 | 54 | 55 | int sm_update_delete_edge (edge_t *edge_in); 56 | 57 | 58 | int sample (state_t **state_sample_out); 59 | 60 | /** 61 | * \brief Updates the trajectory around which the samples should be concentrated. 62 | * 63 | * This function can be called by the user to modify the trajectory around which 64 | * the samples should be concentrated. 65 | * 66 | * @param trajectory_in New trajectory for opra sampling. 67 | * 68 | * @returns Returns 1 for success, a non-positive number for failure. 69 | */ 70 | 71 | 72 | int update_trajectory (trajectory_t *trajectory_in); 73 | 74 | 75 | 76 | /** 77 | * \brief Sets the dimensions and position of the rectangular bounding box of 78 | * the support. 79 | * 80 | * Uniform distribution only makes sense in a bounded support, which can be set 81 | * using this function. This sampler function only draws samples from a rectangular 82 | * box in the Euclidean space with dimensions NUM_DIMENSIONS, which is a template 83 | * parameter to the uniform sampler class. If the support variable is not set, i.e., 84 | * this function is never called, then the support is initialized to the unit cube 85 | * centered at the origin by default. 86 | * 87 | * @param support_in New support for the uniform sampling distribution. 88 | * 89 | * @returns Returns 1 for success, a non-positive number for failure. 90 | */ 91 | int set_support (const region_t support_in); 92 | 93 | 94 | }; 95 | 96 | 97 | } 98 | 99 | -------------------------------------------------------------------------------- /include/smp/components/model_checkers/mu_calculus.h: -------------------------------------------------------------------------------- 1 | /*! \file components/model_checkers/mu_calculus.h 2 | \brief The mu-calculus model checker. 3 | 4 | This includes an implementation of the mu-calculus model checker. 5 | */ 6 | 7 | 8 | #ifndef _SMP_MODEL_CHECKER_MU_CALCULUS_H_ 9 | #define _SMP_MODEL_CHECKER_MU_CALCULUS_H_ 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | 17 | 18 | 19 | namespace smp { 20 | 21 | //! Implements the vertex data for mu-calculus model checking 22 | /*! 23 | The data stored in each vertex of the graph required for the mu-calculus model 24 | checking operation. 25 | */ 26 | class model_checker_mu_calculus_vertex_data { 27 | 28 | public: 29 | 30 | //! State variable for mu-calculus model checking library. 31 | /*! 32 | This variable is a data type used by the external mu-calculus model 33 | checking library. The class holds the set of all sub-formula of the 34 | specification that the associated vertex satisfies. It is incrementally 35 | updated by the procedure. 36 | */ 37 | MS_state *state; 38 | }; 39 | 40 | 41 | //! Implements the edge data for mu-calculus model checking 42 | /*! 43 | This empty class is implemented for the sake of completeness. 44 | */ 45 | class model_checker_mu_calculus_edge_data { 46 | 47 | }; 48 | 49 | 50 | //! Implements the mu-calculus model checker. 51 | /*! 52 | This class inherits from the model_checker_base class. It implements the 53 | mu-calculus model checker using the mu-calculus external libraries that are 54 | included with the smp library. 55 | 56 | \ingroup model_checkers 57 | */ 58 | template< class typeparams > 59 | class model_checker_mu_calculus : public model_checker_base{ 60 | 61 | typedef typename typeparams::state state_t; 62 | typedef typename typeparams::input input_t; 63 | 64 | typedef vertex vertex_t; 65 | typedef edge edge_t; 66 | 67 | typedef trajectory trajectory_t; 68 | 69 | int uid_counter; 70 | 71 | bool found_solution; 72 | 73 | public: 74 | 75 | //! An instance of the mu-calculus model checker external library. 76 | /*! 77 | This variable instantiates the main class of the external library that 78 | carries out the mu-calculus model checking operation. 79 | */ 80 | rModelChecker ms; 81 | 82 | 83 | model_checker_mu_calculus (); 84 | 85 | ~model_checker_mu_calculus (); 86 | 87 | 88 | int mc_update_insert_vertex (vertex_t *vertex_new); 89 | 90 | int mc_update_insert_edge (edge_t *edge_new); 91 | 92 | int mc_update_delete_vertex (vertex_t *vertex_new); 93 | 94 | int mc_update_delete_edge (edge_t *edge_new); 95 | 96 | int get_solution (trajectory_t &trajectory_out); 97 | 98 | }; 99 | 100 | 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /include/smp/components/samplers/theta_star_gaussian.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_GAUSS_HPP_ 2 | #define _SMP_SAMPLER_GAUSS_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | 12 | #include "eigenmultivariatenormal.cpp" 13 | 14 | 15 | template< class typeparams, int NUM_DIMENSIONS > 16 | int smp::theta_star_gaussian 17 | ::sm_update_insert_vertex (vertex_t *vertex_in) { 18 | 19 | return 1; 20 | } 21 | 22 | 23 | template< class typeparams, int NUM_DIMENSIONS > 24 | int smp::theta_star_gaussian 25 | ::sm_update_insert_edge (edge_t *edge_in) { 26 | 27 | return 1; 28 | } 29 | 30 | 31 | template< class typeparams, int NUM_DIMENSIONS > 32 | int smp::theta_star_gaussian 33 | ::sm_update_delete_vertex (vertex_t *vertex_in) { 34 | 35 | return 1; 36 | } 37 | 38 | 39 | template< class typeparams, int NUM_DIMENSIONS > 40 | int smp::theta_star_gaussian 41 | ::sm_update_delete_edge (edge_t *edge_in) { 42 | 43 | return 1; 44 | } 45 | 46 | 47 | template< class typeparams, int NUM_DIMENSIONS > 48 | smp::theta_star_gaussian 49 | ::theta_star_gaussian() { 50 | 51 | // Initialize the sampling distribution support. 52 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 53 | support.center[i] = 0.0; 54 | support.size[i] = 1.0; 55 | } 56 | } 57 | 58 | 59 | template< class typeparams, int NUM_DIMENSIONS > 60 | smp::theta_star_gaussian 61 | ::~theta_star_gaussian () { 62 | 63 | 64 | } 65 | 66 | 67 | 68 | /// Sample function for a Circle Region 69 | template< class typeparams, int NUM_DIMENSIONS > 70 | int smp::theta_star_gaussian 71 | ::sample (state_t **state_sample_out) { 72 | 73 | // Sampling by Using Multivariate Gaussians 74 | Eigen::Vector2d mean; 75 | Eigen::Matrix2d covar; 76 | Eigen::Vector2d ns; 77 | double sigmaxi,sigmayi; 78 | if (NUM_DIMENSIONS <= 0) 79 | return 0; 80 | 81 | state_t *state_new = new state_t; 82 | 83 | mean << support.center[0],support.center[1]; 84 | sigmaxi=1; 85 | sigmayi=1; 86 | Eigen::Matrix2d rot ; 87 | rot=Eigen::Rotation2Dd(0).matrix(); 88 | covar = rot*Eigen::DiagonalMatrix(sigmaxi,sigmayi)*rot.transpose(); 89 | EigenMultivariateNormal *normX = new EigenMultivariateNormal(mean,covar); 90 | (*normX).reseed(rand()+100*std::time(0)*rand()+std::time(0)); 91 | normX->nextSample(ns); 92 | // cout<<"DEBUG: sample --x,y--: -- "< 105 | int smp::theta_star_gaussian 106 | ::set_support (region_t support_in) { 107 | 108 | support = support_in; 109 | 110 | return 1; 111 | } 112 | 113 | 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /include/smp/components/extenders/MotionPrimitiveGenerator.m: -------------------------------------------------------------------------------- 1 | %% MOTION PRIMITIVE GENERATOR 2 | 3 | % THIS SCRIPT GENERATES AND DISPLAYS MOTION PRIMITIVE BASE ON A UNICYCLE 4 | % KINEMATIC MODEL. 5 | % created by lp@srl 26.11.2013 6 | clear all 7 | close all 8 | clc 9 | 10 | 11 | % Define the Velocities' ranges 12 | % WI is the angular velocity range 13 | 14 | 15 | %%% First set of motion primitives 16 | % Wmin=-2.8; 17 | % Wmax=2.8; 18 | % Wstep=0.4; 19 | % wI=Wmin:Wstep:Wmax; 20 | % 21 | % % VI is the translational velocity range 22 | % Vmin=0.5; 23 | % Vmax=4.0; 24 | % Vstep=0.5; 25 | % vI=Vmin:Vstep:Vmax; 26 | 27 | 28 | %% Second set of motion primitives 29 | % Wmin=-1.8; 30 | % Wmax=1.8; 31 | % Wstep=0.3; 32 | % wI=Wmin:Wstep:Wmax; 33 | % 34 | % % VI is the translational velocity range 35 | % Vmin=0.5; 36 | % Vmax=3.0; 37 | % Vstep=0.5; 38 | % vI=Vmin:Vstep:Vmax; 39 | 40 | 41 | % % % %%% Third set of motion primitives 42 | % Wmin=-4.8; 43 | % Wmax=4.8; 44 | % Wstep=0.6; 45 | % wI=Wmin:Wstep:Wmax; 46 | % 47 | % % VI is the translational velocity range 48 | % Vmin=0.5; 49 | % Vmax=4.0; 50 | % Vstep=0.5; 51 | % vI=Vmin:Vstep:Vmax; 52 | 53 | %%% 4th set of motion primitives 54 | % 55 | vI=[0.8 2]; 56 | wI=[-2.6 -1.3 0 1.3 2.6] 57 | 58 | 59 | 60 | k=1; 61 | for i=1:length(vI) 62 | for j=1:length(wI) 63 | 64 | vM(k)=vI(i); 65 | wM(k)=wI(j); 66 | k=k+1; 67 | end 68 | end 69 | 70 | 71 | % define the motion primitive array 72 | motionPrimitiveCommandArray=[vM' wM']; 73 | q0=[0;0;0]; 74 | % Make sure that the time of integration is small!! 75 | ti=0; 76 | tf=0.3; 77 | dt=0.01; 78 | tSpan=ti:dt:tf; 79 | Qexport=[]; 80 | Nexport=[]; 81 | Vexport=[]; 82 | Wexport=[]; 83 | [nr nc]=size(motionPrimitiveCommandArray); 84 | figure(20),hold on 85 | for i=1:nr 86 | v = motionPrimitiveCommandArray(i,1); 87 | omega = motionPrimitiveCommandArray(i,2); 88 | [T Q] = ode45(@(t,q) unicycleKinematics( t, q, v ,omega ),tSpan,q0); % solve ODE 89 | 90 | figure(20), plot(Q(:,1),Q(:,2)) 91 | [n1r nc]=size(Q); 92 | Nexport=[Nexport;n1r]; 93 | Qexport=[Qexport; Q(:,:)]; 94 | Vexport=[Vexport;v]; 95 | Wexport=[Wexport;omega]; 96 | 97 | 98 | end 99 | dir='/media/data/software/pedsimplanning/SGDiCoP/bin/motionPrimitives.txt'; 100 | fid1=fopen(dir,'wt'); 101 | [nr nc]=size(Qexport); 102 | for i=1:nr 103 | fprintf(fid1,'%8.6f %8.6f %8.6f\n',Qexport(i,1),Qexport(i,2),Qexport(i,3)); 104 | end 105 | 106 | fclose(fid1); 107 | 108 | 109 | dir='/media/data/software/pedsimplanning/SGDiCoP/bin/numberMotionPrimitives.txt'; 110 | fid2=fopen(dir,'wt'); 111 | [nre nce]=size(Nexport); 112 | for j=1:nre 113 | fprintf(fid2,'%8.6f \n',Nexport(j)); 114 | end 115 | 116 | fclose(fid2); 117 | 118 | 119 | dir='/media/data/software/pedsimplanning/SGDiCoP/bin/VPrimitives.txt'; 120 | fid3=fopen(dir,'wt'); 121 | [nre nce]=size(Vexport); 122 | for j=1:nre 123 | fprintf(fid3,'%8.6f \n',Vexport(j)); 124 | end 125 | 126 | fclose(fid3); 127 | 128 | 129 | dir='/media/data/software/pedsimplanning/SGDiCoP/bin/WPrimitives.txt'; 130 | fid4=fopen(dir,'wt'); 131 | [nre nce]=size(Wexport); 132 | for j=1:nre 133 | fprintf(fid4,'%8.6f \n',Wexport(j)); 134 | end 135 | 136 | fclose(fid4); 137 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/costotogo_terminate_c.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_terminate.c 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_terminate.c

11 | 
12 |     1   /*
13 |     2    * costotogo_terminate.c
14 |     3    *
15 |     4    * Code generation for function 'costotogo_terminate'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   /* Include files */
22 |    11   #include "rt_nonfinite.h"
23 |    12   #include "costotogo.h"
24 |    13   #include "costotogo_terminate.h"
25 |    14   
26 |    15   /* Function Definitions */
27 |    16   void costotogo_terminate(void)
28 |    17   {
29 |    18     /* (no terminate code required) */
30 |    19   }
31 |    20   
32 |    21   /* End of code generation (costotogo_terminate.c) */
33 |    22
34 |
35 |

36 | 37 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/costotogo_terminate_c.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_terminate.c 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_terminate.c

11 | 
12 |     1   /*
13 |     2    * costotogo_terminate.c
14 |     3    *
15 |     4    * Code generation for function 'costotogo_terminate'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   /* Include files */
22 |    11   #include "rt_nonfinite.h"
23 |    12   #include "costotogo.h"
24 |    13   #include "costotogo_terminate.h"
25 |    14   
26 |    15   /* Function Definitions */
27 |    16   void costotogo_terminate(void)
28 |    17   {
29 |    18     /* (no terminate code required) */
30 |    19   }
31 |    20   
32 |    21   /* End of code generation (costotogo_terminate.c) */
33 |    22
34 |
35 |

36 | 37 | -------------------------------------------------------------------------------- /include/smp/components/extenders/single_integrator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SYSTEM_SINGLE_INTEGRATOR_HPP_ 2 | #define _SMP_SYSTEM_SINGLE_INTEGRATOR_HPP_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | 15 | using namespace std; 16 | 17 | 18 | template< class typeparams, int NUM_DIMENSIONS > 19 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 20 | ::ex_update_insert_vertex (vertex_t *vertex_in) { 21 | 22 | return 1; 23 | } 24 | 25 | 26 | template< class typeparams, int NUM_DIMENSIONS > 27 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 28 | ::ex_update_insert_edge (edge_t *edge_in) { 29 | 30 | return 1; 31 | } 32 | 33 | 34 | template< class typeparams, int NUM_DIMENSIONS > 35 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 36 | ::ex_update_delete_vertex (vertex_t *vertex_in){ 37 | 38 | return 1; 39 | } 40 | 41 | 42 | template< class typeparams, int NUM_DIMENSIONS > 43 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 44 | ::ex_update_delete_edge (edge_t *edge_in) { 45 | 46 | return 1; 47 | } 48 | 49 | 50 | template< class typeparams, int NUM_DIMENSIONS > 51 | smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 52 | ::extender_single_integrator () { 53 | 54 | max_length = 1.0; 55 | } 56 | 57 | 58 | template< class typeparams, int NUM_DIMENSIONS > 59 | smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 60 | ::~extender_single_integrator () { 61 | 62 | } 63 | 64 | template< class typeparams, int NUM_DIMENSIONS > 65 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 66 | ::set_max_length (double max_length_in) { 67 | 68 | if (max_length_in <= 0.0) 69 | return 0; 70 | 71 | max_length = max_length_in; 72 | 73 | return 1; 74 | } 75 | 76 | 77 | template< class typeparams, int NUM_DIMENSIONS > 78 | int smp::extender_single_integrator< typeparams, NUM_DIMENSIONS > 79 | ::extend (state_t *state_from_in, state_t *state_towards_in, 80 | int *exact_connection_out, trajectory_t *trajectory_out, 81 | list *intermediate_vertices_out) { 82 | 83 | if (max_length <= 0.0) 84 | return 0; 85 | 86 | trajectory_out->list_states.clear(); 87 | trajectory_out->list_inputs.clear(); 88 | intermediate_vertices_out->clear(); 89 | 90 | double dists[NUM_DIMENSIONS]; 91 | double dist = 0.0; 92 | 93 | 94 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 95 | dists[i] = (*state_towards_in)[i] - (*state_from_in)[i]; 96 | dist += dists[i] * dists[i]; 97 | } 98 | dist = sqrt(dist); 99 | 100 | 101 | state_t *state_new; 102 | input_t *input_new = new input_t; 103 | 104 | 105 | if (dist < max_length) { 106 | 107 | state_new = new state_t (*state_towards_in); 108 | (*input_new)[0] = dist; 109 | *exact_connection_out = 1; 110 | } 111 | else { 112 | 113 | state_new = new state_t; 114 | for (int i = 0; i < NUM_DIMENSIONS; i++) 115 | (*state_new)[i] = (*state_from_in)[i] + dists[i]/dist*max_length; 116 | (*input_new)[0] = max_length; 117 | *exact_connection_out = 0; 118 | } 119 | 120 | trajectory_out->list_states.push_back (state_new); 121 | trajectory_out->list_inputs.push_back (input_new); 122 | 123 | return 1; 124 | } 125 | 126 | 127 | #endif 128 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/costotogo_initialize_c.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_initialize.c 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_initialize.c

11 | 
12 |     1   /*
13 |     2    * costotogo_initialize.c
14 |     3    *
15 |     4    * Code generation for function 'costotogo_initialize'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   /* Include files */
22 |    11   #include "rt_nonfinite.h"
23 |    12   #include "costotogo.h"
24 |    13   #include "costotogo_initialize.h"
25 |    14   
26 |    15   /* Function Definitions */
27 |    16   void costotogo_initialize(void)
28 |    17   {
29 |    18     rt_InitInfAndNaN(8U);
30 |    19   }
31 |    20   
32 |    21   /* End of code generation (costotogo_initialize.c) */
33 |    22
34 |
35 |

36 | 37 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/costotogo_initialize_c.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | RTW Report - costotogo_initialize.c 5 | 6 | 7 | 8 |

9 |
10 |

File: costotogo_initialize.c

11 | 
12 |     1   /*
13 |     2    * costotogo_initialize.c
14 |     3    *
15 |     4    * Code generation for function 'costotogo_initialize'
16 |     5    *
17 |     6    * C source code generated on: Wed Jun 11 17:43:10 2014
18 |     7    *
19 |     8    */
20 |     9   
21 |    10   /* Include files */
22 |    11   #include "rt_nonfinite.h"
23 |    12   #include "costotogo.h"
24 |    13   #include "costotogo_initialize.h"
25 |    14   
26 |    15   /* Function Definitions */
27 |    16   void costotogo_initialize(void)
28 |    17   {
29 |    18     rt_InitInfAndNaN(8U);
30 |    19   }
31 |    20   
32 |    21   /* End of code generation (costotogo_initialize.c) */
33 |    22
34 |
35 |

36 | 37 | -------------------------------------------------------------------------------- /include/smp/planners/rrt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_PLANNER_RRT_HPP_ 2 | #define _SMP_PLANNER_RRT_HPP_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | 9 | 10 | template< class typeparams > 11 | smp::rrt 12 | ::rrt () { 13 | 14 | 15 | } 16 | 17 | 18 | 19 | template< class typeparams > 20 | smp::rrt 21 | ::~rrt () { 22 | 23 | 24 | } 25 | 26 | 27 | template< class typeparams > 28 | smp::rrt 29 | ::rrt (sampler_t &sampler_in, distance_evaluator_t &distance_evaluator_in, extender_t &extender_in, 30 | collision_checker_t &collision_checker_in, model_checker_t &model_checker_in) : 31 | planner_incremental(sampler_in, distance_evaluator_in, extender_in, collision_checker_in, model_checker_in) { 32 | 33 | 34 | } 35 | 36 | 37 | template< class typeparams > 38 | int smp::rrt 39 | ::iteration () { 40 | 41 | // TODO: Check whether the rrt is initialized properly (including its base classes) 42 | 43 | // 1. Sample a new state from the obstacle-free space 44 | state_t *state_sample; 45 | this->sampler.sample (&state_sample); 46 | if (this->collision_checker.check_collision_state (state_sample) == 0) { 47 | delete state_sample; 48 | return 0; 49 | } 50 | 51 | 52 | x=state_sample->state_vars[0]; 53 | y=state_sample->state_vars[1]; 54 | z=state_sample->state_vars[2]; 55 | 56 | // 2. Find the nearest vertex 57 | vertex_t *vertex_nearest; 58 | this->distance_evaluator.find_nearest_vertex (state_sample, (void **)&vertex_nearest); 59 | 60 | 61 | // 3. Extend the nearest vertex towards the sample 62 | int exact_connection = -1; 63 | trajectory_t *trajectory = new trajectory_t; 64 | list *intermediate_vertices = new list; 65 | if (this->extender.extend (vertex_nearest->state, state_sample, 66 | &exact_connection, trajectory, intermediate_vertices) == 1) { // If the extension is successful 67 | // 4. Check the new trajectory for collision 68 | if (this->check_extended_trajectory_for_collision (vertex_nearest->state, trajectory) == 1) { // If collision free 69 | 70 | 71 | vertex_t *vertex_parent = vertex_nearest; 72 | trajectory_t *trajectory_parent = trajectory; 73 | list *intermediate_vertices_parent = intermediate_vertices; 74 | 75 | double cost_trajectory_from_parent = this->min_time_reachability.evaluate_cost_trajectory (vertex_parent->state, trajectory_parent); 76 | double cost_parent = vertex_parent->data.total_cost + cost_trajectory_from_parent; 77 | 78 | // 5. Add the new collision-free trajectory to the tree 79 | this->insert_trajectory (vertex_nearest, trajectory, intermediate_vertices); 80 | 81 | // Update the cost of the edge and the vertex 82 | vertex_t *vertex_last = this->list_vertices.back(); 83 | vertex_last->data.total_cost = cost_parent; 84 | this->min_time_reachability.ce_update_vertex_cost (vertex_last); 85 | 86 | edge_t *edge_last = vertex_parent->outgoing_edges.back(); 87 | edge_last->data.edge_cost = cost_trajectory_from_parent; 88 | 89 | // Exit with success 90 | delete state_sample; 91 | return 1; 92 | } 93 | } 94 | 95 | 96 | // 6. Handle the error case 97 | // If the extension was not successful, or the trajectory was not collision free, 98 | // then free the memory and return failure. 99 | delete state_sample; 100 | delete trajectory; 101 | delete intermediate_vertices; 102 | 103 | // Exit with error 104 | return 0; 105 | 106 | } 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /include/smp/costtogo/html/contents_file.tmp: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 |
2 | Generated Source Files 3 |
costotogo.c
costotogo.h
costotogo_initialize.c
costotogo_initialize.h
costotogo_terminate.c
costotogo_terminate.h
costotogo_types.h
rtGetInf.c
rtGetInf.h
rtGetNaN.c
rtGetNaN.h
rt_nonfinite.c
rt_nonfinite.h
rtwtypes.h
19 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/html/contents_file.tmp: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 |
2 | Generated Source Files 3 |
costotogo.c
costotogo.h
costotogo_initialize.c
costotogo_initialize.h
costotogo_terminate.c
costotogo_terminate.h
costotogo_types.h
rtGetInf.c
rtGetInf.h
rtGetNaN.c
rtGetNaN.h
rt_nonfinite.c
rt_nonfinite.h
rtwtypes.h
19 | -------------------------------------------------------------------------------- /include/smp/components/extenders/dubins.h: -------------------------------------------------------------------------------- 1 | /*! \file components/extenders/dubins.h 2 | \brief The extend function component that implements a dubins car. 3 | 4 | The extender that this file implements an extender based on the 5 | dubins car dynamical system (the dimensionality of the state space 6 | is a template parameter) that exactly or approximately connects two given states. 7 | */ 8 | 9 | 10 | #ifndef _SMP_SYSTEM_DUBINS_H_ 11 | #define _SMP_SYSTEM_DUBINS_H_ 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | using namespace std; 21 | 22 | 23 | namespace smp { 24 | 25 | //! Implementation of the state data structure for the Dubins car dynamics 26 | /*! 27 | This class implements the state data structure for the Dubins car dynamics. 28 | The number of state variables is three. The state variables indicate position 29 | in the x and y coordinates and the orientation, in this order. 30 | 31 | \ingroup states 32 | */ 33 | class state_dubins : public state_array_double <3> { 34 | 35 | }; 36 | 37 | 38 | 39 | //! Implementation of the input data structure for the Dubins car dynamics. 40 | /*! 41 | This class implements the input data structure for teh Dubins car dynamics. 42 | The number of input variables is exactly two. The first input variable 43 | stores the time it takes to execute the trajectory segment, while the second 44 | variable stores the steering input required. 45 | 46 | \ingroup inputs 47 | */ 48 | class input_dubins : public input_array_double<2> { 49 | 50 | 51 | }; 52 | 53 | 54 | //! Implements the extender function with Dubins car dynamics. 55 | /*! 56 | This class implements an extender with the Dubins car dynamics. 57 | 58 | \ingroup extenders 59 | */ 60 | template< class typeparams > 61 | class extender_dubins : public extender_base { 62 | 63 | 64 | 65 | typedef typename typeparams::state state_t; 66 | typedef typename typeparams::input input_t; 67 | typedef typename typeparams::vertex_data vertex_data_t; 68 | typedef typename typeparams::edge_data edge_data_t; 69 | 70 | typedef vertex vertex_t; 71 | typedef edge edge_t; 72 | 73 | typedef trajectory trajectory_t; 74 | 75 | int extend_dubins_spheres (double x_s1, double y_s1, double t_s1, 76 | double x_s2, double y_s2, double t_s2, int comb_no, 77 | int *fully_extends, list *list_states, list *list_inputs); 78 | 79 | double extend_dubins_all (state_t *state_ini, state_t *state_fin, 80 | int *fully_extends, list *list_states_out, list *list_inputs_out); 81 | 82 | 83 | public : 84 | 85 | extender_dubins (); 86 | ~extender_dubins (); 87 | 88 | 89 | int ex_update_insert_vertex (vertex_t *vertex_in); 90 | 91 | 92 | int ex_update_insert_edge (edge_t *edge_in); 93 | 94 | 95 | int ex_update_delete_vertex (vertex_t *vertex_in); 96 | 97 | 98 | int ex_update_delete_edge (edge_t *edge_in); 99 | 100 | 101 | int extend (state_t *state_from_in, state_t *state_towards_in, 102 | int *exact_connection_out, trajectory_t *trajectory_out, 103 | list *intermediate_vertices_out); 104 | 105 | }; 106 | 107 | 108 | } 109 | 110 | #endif 111 | -------------------------------------------------------------------------------- /include/smp/components/cost_evaluators/base.h: -------------------------------------------------------------------------------- 1 | /*! \file components/cost_evaluators/base.h 2 | \brief The abstract cost evaluator 3 | 4 | This file provides an implementation of the abstract class for the 5 | generic cost evaluator. 6 | */ 7 | 8 | #ifndef _SMP_COST_EVALUATOR_BASE_H_ 9 | #define _SMP_COST_EVALUATOR_BASE_H_ 10 | 11 | #include 12 | #include 13 | 14 | 15 | namespace smp { 16 | 17 | //! The abstract class that specifies the structure of the cost evalutor component. 18 | /*! 19 | This class implements the abstract cost evaluator class, which provides one main 20 | method that returns the cost of a trajectory starting from a given initial state 21 | a reaching a given final vertex. 22 | 23 | \ingroup cost_evaluators 24 | */ 25 | template< class typeparams > 26 | class cost_evaluator_base { 27 | 28 | typedef typename typeparams::state state_t; 29 | typedef typename typeparams::input input_t; 30 | typedef typename typeparams::vertex_data vertex_data_t; 31 | typedef typename typeparams::edge_data edge_data_t; 32 | 33 | typedef vertex vertex_t; 34 | typedef edge edge_t; 35 | 36 | typedef trajectory trajectory_t; 37 | 38 | public: 39 | 40 | /** 41 | * \brief Choose how the distance metric can be computed 42 | * 43 | * SELECT_FUNC == 0. True Distance Metric 44 | * SELECT_FUNC == 1. regression_nlm 45 | * SELECT_FUNC == 2. regression_nn 46 | */ 47 | 48 | int SELECT_FUNC; 49 | 50 | /** 51 | * \brief Update function for vertex cost modification 52 | * 53 | * This function is called by the planner whenever a the cost associated 54 | * with a vertex is changed by the optimizing (incremental) planning algorithm. 55 | * 56 | * @param vertex_in A pointer to the vertex with modified cost. 57 | * 58 | * @returns Return 1 if success, a non-positive value to indiacate error. 59 | */ 60 | virtual int ce_update_vertex_cost (vertex_t *vertex_in) = 0; 61 | 62 | 63 | /** 64 | * \brief Update function for edge cost modification 65 | * 66 | * This function is called by the planner whenever the cost associated 67 | * with an edge is changed by the optimizing (incremental) planning algorithm. 68 | * 69 | * @param edge_in A pointer to the edge with modified cost. 70 | * 71 | * @returns Return 1 if success, a non-positive value to indiacate error. 72 | */ 73 | virtual int ce_update_edge_cost (edge_t *edge_in) = 0; 74 | 75 | 76 | /** 77 | * \brief Evaluates the cost of a trajectory. 78 | * 79 | * This function returns the cost of a given trajectory that starts from 80 | * state_initial_in and reaches state_final_in. Sometimes the final state 81 | * is embedded in the trajectory itself, in which case state_final_in argument 82 | * can be set to NULL. 83 | * 84 | * @param state_initial_in Initial state that the trajectory starts from 85 | * @param trajectory_in Trajectory 86 | * @param state_final_in Final state that the trajectory reaches 87 | * 88 | * @returns Returns 1 for success, and a non-positive number to indicate error. 89 | */ 90 | virtual double evaluate_cost_trajectory (state_t *state_initial_in, 91 | trajectory_t *trajectory_in, 92 | state_t *state_final_in = 0) = 0; 93 | 94 | }; 95 | 96 | 97 | } 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /include/smp/costtogo/rtGetInf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetInf.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finite, Inf and MinusInf 13 | */ 14 | #include "rtGetInf.h" 15 | #define NumBitsPerChar 8U 16 | 17 | /* Function: rtGetInf ================================================== 18 | * Abstract: 19 | * Initialize rtInf needed by the generated code. 20 | * Inf is initialized as non-signaling. Assumes IEEE. 21 | */ 22 | real_T rtGetInf(void) 23 | { 24 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 25 | real_T inf = 0.0; 26 | if (bitsPerReal == 32U) { 27 | inf = rtGetInfF(); 28 | } else { 29 | uint16_T one = 1U; 30 | enum { 31 | LittleEndian, 32 | BigEndian 33 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 34 | switch (machByteOrder) { 35 | case LittleEndian: 36 | { 37 | union { 38 | LittleEndianIEEEDouble bitVal; 39 | real_T fltVal; 40 | } tmpVal; 41 | 42 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 43 | tmpVal.bitVal.words.wordL = 0x00000000U; 44 | inf = tmpVal.fltVal; 45 | break; 46 | } 47 | 48 | case BigEndian: 49 | { 50 | union { 51 | BigEndianIEEEDouble bitVal; 52 | real_T fltVal; 53 | } tmpVal; 54 | 55 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 56 | tmpVal.bitVal.words.wordL = 0x00000000U; 57 | inf = tmpVal.fltVal; 58 | break; 59 | } 60 | } 61 | } 62 | 63 | return inf; 64 | } 65 | 66 | /* Function: rtGetInfF ================================================== 67 | * Abstract: 68 | * Initialize rtInfF needed by the generated code. 69 | * Inf is initialized as non-signaling. Assumes IEEE. 70 | */ 71 | real32_T rtGetInfF(void) 72 | { 73 | IEEESingle infF; 74 | infF.wordL.wordLuint = 0x7F800000U; 75 | return infF.wordL.wordLreal; 76 | } 77 | 78 | /* Function: rtGetMinusInf ================================================== 79 | * Abstract: 80 | * Initialize rtMinusInf needed by the generated code. 81 | * Inf is initialized as non-signaling. Assumes IEEE. 82 | */ 83 | real_T rtGetMinusInf(void) 84 | { 85 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 86 | real_T minf = 0.0; 87 | if (bitsPerReal == 32U) { 88 | minf = rtGetMinusInfF(); 89 | } else { 90 | uint16_T one = 1U; 91 | enum { 92 | LittleEndian, 93 | BigEndian 94 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 95 | switch (machByteOrder) { 96 | case LittleEndian: 97 | { 98 | union { 99 | LittleEndianIEEEDouble bitVal; 100 | real_T fltVal; 101 | } tmpVal; 102 | 103 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 104 | tmpVal.bitVal.words.wordL = 0x00000000U; 105 | minf = tmpVal.fltVal; 106 | break; 107 | } 108 | 109 | case BigEndian: 110 | { 111 | union { 112 | BigEndianIEEEDouble bitVal; 113 | real_T fltVal; 114 | } tmpVal; 115 | 116 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 117 | tmpVal.bitVal.words.wordL = 0x00000000U; 118 | minf = tmpVal.fltVal; 119 | break; 120 | } 121 | } 122 | } 123 | 124 | return minf; 125 | } 126 | 127 | /* Function: rtGetMinusInfF ================================================== 128 | * Abstract: 129 | * Initialize rtMinusInfF needed by the generated code. 130 | * Inf is initialized as non-signaling. Assumes IEEE. 131 | */ 132 | real32_T rtGetMinusInfF(void) 133 | { 134 | IEEESingle minfF; 135 | minfF.wordL.wordLuint = 0xFF800000U; 136 | return minfF.wordL.wordLreal; 137 | } 138 | 139 | /* End of code generation (rtGetInf.c) */ 140 | -------------------------------------------------------------------------------- /include/smp/components/multipurpose/costtogo/rtGetInf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * rtGetInf.c 3 | * 4 | * Code generation for function 'costotogo' 5 | * 6 | * C source code generated on: Wed Jun 11 17:43:10 2014 7 | * 8 | */ 9 | 10 | /* 11 | * Abstract: 12 | * MATLAB for code generation function to initialize non-finite, Inf and MinusInf 13 | */ 14 | #include "rtGetInf.h" 15 | #define NumBitsPerChar 8U 16 | 17 | /* Function: rtGetInf ================================================== 18 | * Abstract: 19 | * Initialize rtInf needed by the generated code. 20 | * Inf is initialized as non-signaling. Assumes IEEE. 21 | */ 22 | real_T rtGetInf(void) 23 | { 24 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 25 | real_T inf = 0.0; 26 | if (bitsPerReal == 32U) { 27 | inf = rtGetInfF(); 28 | } else { 29 | uint16_T one = 1U; 30 | enum { 31 | LittleEndian, 32 | BigEndian 33 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 34 | switch (machByteOrder) { 35 | case LittleEndian: 36 | { 37 | union { 38 | LittleEndianIEEEDouble bitVal; 39 | real_T fltVal; 40 | } tmpVal; 41 | 42 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 43 | tmpVal.bitVal.words.wordL = 0x00000000U; 44 | inf = tmpVal.fltVal; 45 | break; 46 | } 47 | 48 | case BigEndian: 49 | { 50 | union { 51 | BigEndianIEEEDouble bitVal; 52 | real_T fltVal; 53 | } tmpVal; 54 | 55 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 56 | tmpVal.bitVal.words.wordL = 0x00000000U; 57 | inf = tmpVal.fltVal; 58 | break; 59 | } 60 | } 61 | } 62 | 63 | return inf; 64 | } 65 | 66 | /* Function: rtGetInfF ================================================== 67 | * Abstract: 68 | * Initialize rtInfF needed by the generated code. 69 | * Inf is initialized as non-signaling. Assumes IEEE. 70 | */ 71 | real32_T rtGetInfF(void) 72 | { 73 | IEEESingle infF; 74 | infF.wordL.wordLuint = 0x7F800000U; 75 | return infF.wordL.wordLreal; 76 | } 77 | 78 | /* Function: rtGetMinusInf ================================================== 79 | * Abstract: 80 | * Initialize rtMinusInf needed by the generated code. 81 | * Inf is initialized as non-signaling. Assumes IEEE. 82 | */ 83 | real_T rtGetMinusInf(void) 84 | { 85 | size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); 86 | real_T minf = 0.0; 87 | if (bitsPerReal == 32U) { 88 | minf = rtGetMinusInfF(); 89 | } else { 90 | uint16_T one = 1U; 91 | enum { 92 | LittleEndian, 93 | BigEndian 94 | } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; 95 | switch (machByteOrder) { 96 | case LittleEndian: 97 | { 98 | union { 99 | LittleEndianIEEEDouble bitVal; 100 | real_T fltVal; 101 | } tmpVal; 102 | 103 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 104 | tmpVal.bitVal.words.wordL = 0x00000000U; 105 | minf = tmpVal.fltVal; 106 | break; 107 | } 108 | 109 | case BigEndian: 110 | { 111 | union { 112 | BigEndianIEEEDouble bitVal; 113 | real_T fltVal; 114 | } tmpVal; 115 | 116 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 117 | tmpVal.bitVal.words.wordL = 0x00000000U; 118 | minf = tmpVal.fltVal; 119 | break; 120 | } 121 | } 122 | } 123 | 124 | return minf; 125 | } 126 | 127 | /* Function: rtGetMinusInfF ================================================== 128 | * Abstract: 129 | * Initialize rtMinusInfF needed by the generated code. 130 | * Inf is initialized as non-signaling. Assumes IEEE. 131 | */ 132 | real32_T rtGetMinusInfF(void) 133 | { 134 | IEEESingle minfF; 135 | minfF.wordL.wordLuint = 0xFF800000U; 136 | return minfF.wordL.wordLreal; 137 | } 138 | 139 | /* End of code generation (rtGetInf.c) */ 140 | -------------------------------------------------------------------------------- /include/smp/components/samplers/uniform.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _SMP_SAMPLER_UNIFORM_HPP_ 2 | #define _SMP_SAMPLER_UNIFORM_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | //#include 12 | 13 | 14 | template< class typeparams, int NUM_DIMENSIONS > 15 | int smp::sampler_uniform 16 | ::sm_update_insert_vertex (vertex_t *vertex_in) { 17 | 18 | return 1; 19 | } 20 | 21 | 22 | template< class typeparams, int NUM_DIMENSIONS > 23 | int smp::sampler_uniform 24 | ::sm_update_insert_edge (edge_t *edge_in) { 25 | 26 | return 1; 27 | } 28 | 29 | 30 | template< class typeparams, int NUM_DIMENSIONS > 31 | int smp::sampler_uniform 32 | ::sm_update_delete_vertex (vertex_t *vertex_in) { 33 | 34 | return 1; 35 | } 36 | 37 | 38 | template< class typeparams, int NUM_DIMENSIONS > 39 | int smp::sampler_uniform 40 | ::sm_update_delete_edge (edge_t *edge_in) { 41 | 42 | return 1; 43 | } 44 | 45 | 46 | template< class typeparams, int NUM_DIMENSIONS > 47 | smp::sampler_uniform 48 | ::sampler_uniform () { 49 | 50 | // Initialize the sampling distribution support. 51 | for (int i = 0; i < NUM_DIMENSIONS; i++) { 52 | support.center[i] = 0.0; 53 | support.size[i] = 1.0; 54 | } 55 | GOAL_BIASING=0; 56 | /// initialize random number generator 57 | srand (102); // set to time(NULL) for change the seed number in every motion planning problem 58 | 59 | } 60 | 61 | 62 | template< class typeparams, int NUM_DIMENSIONS > 63 | smp::sampler_uniform 64 | ::~sampler_uniform () { 65 | 66 | 67 | } 68 | 69 | 70 | template< class typeparams, int NUM_DIMENSIONS > 71 | int smp::sampler_uniform 72 | ::sample (state_t **state_sample_out) { 73 | 74 | if (NUM_DIMENSIONS <= 0) 75 | 76 | 0; 77 | 78 | state_t *state_new = new state_t; 79 | 80 | if(GOAL_BIASING){ 81 | 82 | double p; 83 | p=rand()/(RAND_MAX + 1.0)-1/2; 84 | 85 | if(p 121 | int smp::sampler_uniform 122 | ::set_support (region_t support_in) { 123 | 124 | support = support_in; 125 | 126 | return 1; 127 | } 128 | 129 | template< class typeparams, int NUM_DIMENSIONS > 130 | int smp::sampler_uniform 131 | ::set_goal(region_t g){ 132 | 133 | this->goal=g; 134 | 135 | } 136 | 137 | template< class typeparams, int NUM_DIMENSIONS > 138 | int smp::sampler_uniform 139 | ::set_goal_biasing(int option){ 140 | 141 | this->GOAL_BIASING=option; 142 | 143 | } 144 | 145 | 146 | template< class typeparams, int NUM_DIMENSIONS > 147 | int smp::sampler_uniform 148 | ::set_goal_biasing_ths(double p){ 149 | 150 | this->P_THS=p; 151 | 152 | } 153 | 154 | #endif 155 | --------------------------------------------------------------------------------