├── qp_matrices ├── tuning │ ├── R.txt │ ├── u_constr.txt │ ├── Qlin.txt │ ├── Qrate_Qdiff.txt │ ├── Qstate_rate.txt │ ├── Q.txt │ ├── Qf.txt │ ├── x_constr.txt │ └── Qerr.txt └── planningModelMatrices │ ├── Blinear.txt │ └── Alinear.txt ├── msg ├── sensor.msg ├── cmd.msg ├── cmdDrone.msg ├── input.msg ├── linearMatrices.msg ├── highLevelBelief.msg ├── state.msg ├── lowLevelLog.msg ├── optSol.msg ├── goalSetAndState.msg ├── valFunCnst.msg └── stateDrone.msg ├── .gitignore ├── drone_qp_matrices └── qp_matrices │ ├── tuning │ ├── R.txt │ ├── Qlin.txt │ ├── Qrate_Qdiff.txt │ ├── u_constr.txt │ ├── Q.txt │ ├── Qf.txt │ ├── Qstate_rate.txt │ ├── Qerr.txt │ └── x_constr.txt │ └── planningModelMatrices │ ├── Blinear.txt │ └── Alinear.txt ├── srv └── ui.srv ├── include ├── Eigen_embedded │ ├── Eigen │ ├── src │ │ ├── misc │ │ │ ├── lapacke.h │ │ │ ├── lapacke_mangling.h │ │ │ ├── RealSvd2x2.h │ │ │ ├── Kernel.h │ │ │ └── Image.h │ │ ├── Core │ │ │ ├── util │ │ │ │ ├── NonMPL2.h │ │ │ │ ├── ReenableStupidWarnings.h │ │ │ │ └── DisableStupidWarnings.h │ │ │ ├── functors │ │ │ │ └── TernaryFunctors.h │ │ │ ├── DiagonalProduct.h │ │ │ ├── arch │ │ │ │ ├── AVX │ │ │ │ │ └── TypeCasting.h │ │ │ │ ├── Default │ │ │ │ │ ├── Settings.h │ │ │ │ │ └── ConjHelper.h │ │ │ │ ├── SSE │ │ │ │ │ └── TypeCasting.h │ │ │ │ ├── CUDA │ │ │ │ │ └── MathFunctions.h │ │ │ │ └── NEON │ │ │ │ │ └── MathFunctions.h │ │ │ ├── SelfCwiseBinaryOp.h │ │ │ ├── Swap.h │ │ │ ├── Assign.h │ │ │ ├── VectorBlock.h │ │ │ ├── MathFunctionsImpl.h │ │ │ ├── NestByValue.h │ │ │ ├── Inverse.h │ │ │ └── NoAlias.h │ │ ├── SparseCore │ │ │ ├── SparseFuzzy.h │ │ │ ├── SparseRedux.h │ │ │ ├── MappedSparseMatrix.h │ │ │ ├── SparseDot.h │ │ │ └── SparseTranspose.h │ │ ├── SparseLU │ │ │ ├── SparseLU_Utils.h │ │ │ ├── SparseLU_relax_snode.h │ │ │ └── SparseLU_copy_to_ucol.h │ │ ├── StlSupport │ │ │ └── details.h │ │ ├── QR │ │ │ └── HouseholderQR_LAPACKE.h │ │ ├── plugins │ │ │ └── MatrixCwiseUnaryOps.h │ │ ├── LU │ │ │ ├── Determinant.h │ │ │ └── PartialPivLU_LAPACKE.h │ │ ├── Eigenvalues │ │ │ └── RealSchur_LAPACKE.h │ │ └── Geometry │ │ │ └── EulerAngles.h │ ├── Dense │ ├── CMakeLists.txt │ ├── StdList │ ├── StdDeque │ ├── StdVector │ ├── Householder │ ├── Sparse │ ├── Jacobi │ ├── QtAlignedMalloc │ ├── MetisSupport │ ├── PardisoSupport │ ├── SPQRSupport │ ├── SparseQR │ ├── Cholesky │ ├── QR │ ├── UmfPackSupport │ ├── SparseCholesky │ ├── LU │ ├── SVD │ ├── PaStiXSupport │ ├── SparseLU │ ├── Eigenvalues │ ├── CholmodSupport │ ├── IterativeLinearSolvers │ ├── Geometry │ ├── SuperLUSupport │ ├── SparseCore │ └── OrderingMethods ├── segway_sim │ ├── sensor_node.hpp │ ├── controller_node.hpp │ ├── low_level_node.hpp │ ├── ekf.h │ ├── keyboard.hpp │ ├── manual_teleop_node.hpp │ ├── integrator_node.hpp │ ├── mpc_node.hpp │ ├── mpc_drone_node.hpp │ └── CyberTimer.hpp └── Eigen_embedded.h ├── URDF ├── Frame.stl ├── WheelL.stl ├── WheelR.stl └── cyberpod.urdf ├── plotUtils ├── prob.gif ├── test.npy ├── barrier.npy ├── compTime.gif ├── obstBelief.npy └── plotGif.py ├── src ├── workspace │ └── .metadata │ │ └── .plugins │ │ ├── org.eclipse.core.resources │ │ └── .root │ │ │ └── .indexes │ │ │ └── properties.version │ │ └── org.eclipse.core.runtime │ │ └── .settings │ │ ├── org.eclipse.ui.prefs │ │ ├── org.eclipse.core.resources.prefs │ │ └── com.digi.xctung.prefs ├── pyFun │ ├── MOMDP.pyc │ ├── utils.pyc │ ├── __pycache__ │ │ ├── MOMDP.cpython-37.pyc │ │ └── utils.cpython-37.pyc │ ├── pyMOMDP.yml │ └── mainMulti.py ├── keyboard.cpp ├── ekf.cpp ├── ekf_node.cpp ├── sensor_node.cpp ├── controller_node.cpp └── manual_teleop_node.cpp ├── workspace └── .metadata │ ├── .plugins │ └── org.eclipse.core.runtime │ │ └── .settings │ │ ├── org.eclipse.ui.prefs │ │ └── com.digi.xctung.prefs │ └── .log ├── launch ├── start_momdp.launch ├── README.md ├── highLevelDrone.launch ├── highLevelMulti.launch ├── highLevelMulti_hw.launch ├── manual_teleop.launch ├── rviz.launch ├── drone_hardware.launch ├── mpcDrone.launch ├── rvizMultiAgent.launch ├── hardware_mpc.launch ├── moving_obstacle.launch └── main.launch ├── package.xml ├── LICENSE └── README.md /qp_matrices/tuning/R.txt: -------------------------------------------------------------------------------- 1 | .1 .1 2 | -------------------------------------------------------------------------------- /qp_matrices/tuning/u_constr.txt: -------------------------------------------------------------------------------- 1 | 15 15 -------------------------------------------------------------------------------- /qp_matrices/tuning/Qlin.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 2 | -------------------------------------------------------------------------------- /qp_matrices/tuning/Qrate_Qdiff.txt: -------------------------------------------------------------------------------- 1 | .1 1.0 2 | -------------------------------------------------------------------------------- /qp_matrices/tuning/Qstate_rate.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 -------------------------------------------------------------------------------- /msg/sensor.msg: -------------------------------------------------------------------------------- 1 | uint32 time 2 | float32[] data 3 | -------------------------------------------------------------------------------- /qp_matrices/tuning/Q.txt: -------------------------------------------------------------------------------- 1 | 0.1 0.1 0.0 0.0 10.0 1.0 10.0 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **.pyc 2 | *.pyc 3 | *.obj 4 | *.pkl 5 | *.xmi -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/R.txt: -------------------------------------------------------------------------------- 1 | 0.1 0.1 2 | -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Qlin.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 2 | -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Qrate_Qdiff.txt: -------------------------------------------------------------------------------- 1 | 0.1 0 2 | -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/u_constr.txt: -------------------------------------------------------------------------------- 1 | 10 10 2 | -------------------------------------------------------------------------------- /msg/cmd.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64[] cmd -------------------------------------------------------------------------------- /qp_matrices/tuning/Qf.txt: -------------------------------------------------------------------------------- 1 | 100.0 100.0 0.0 1.0 100.0 100.0 200.0 -------------------------------------------------------------------------------- /srv/ui.srv: -------------------------------------------------------------------------------- 1 | uint8 cmd 2 | float64[] data 3 | --- 4 | bool result -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Q.txt: -------------------------------------------------------------------------------- 1 | 100 100 2000 2000 2 | -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Qf.txt: -------------------------------------------------------------------------------- 1 | 1000 1000 1000 1000 2 | -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Qstate_rate.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 -------------------------------------------------------------------------------- /msg/cmdDrone.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64[4] vDes -------------------------------------------------------------------------------- /msg/input.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64[2] inputVec -------------------------------------------------------------------------------- /qp_matrices/tuning/x_constr.txt: -------------------------------------------------------------------------------- 1 | 10 10 60.2832 5 31.416 6.2832 31.416 -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/Qerr.txt: -------------------------------------------------------------------------------- 1 | 0.0 0.0 0.0 0.0 0.0 0.0 0.0 -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/tuning/x_constr.txt: -------------------------------------------------------------------------------- 1 | 100 100 1.5 1.5 2 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /URDF/Frame.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/URDF/Frame.stl -------------------------------------------------------------------------------- /URDF/WheelL.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/URDF/WheelL.stl -------------------------------------------------------------------------------- /URDF/WheelR.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/URDF/WheelR.stl -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/planningModelMatrices/Blinear.txt: -------------------------------------------------------------------------------- 1 | 0 0 2 | 0 0 3 | 1 0 4 | 0 1 5 | -------------------------------------------------------------------------------- /plotUtils/prob.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/plotUtils/prob.gif -------------------------------------------------------------------------------- /plotUtils/test.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/plotUtils/test.npy -------------------------------------------------------------------------------- /src/workspace/.metadata/.plugins/org.eclipse.core.resources/.root/.indexes/properties.version: -------------------------------------------------------------------------------- 1 |  -------------------------------------------------------------------------------- /plotUtils/barrier.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/plotUtils/barrier.npy -------------------------------------------------------------------------------- /src/pyFun/MOMDP.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/src/pyFun/MOMDP.pyc -------------------------------------------------------------------------------- /src/pyFun/utils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/src/pyFun/utils.pyc -------------------------------------------------------------------------------- /plotUtils/compTime.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/plotUtils/compTime.gif -------------------------------------------------------------------------------- /drone_qp_matrices/qp_matrices/planningModelMatrices/Alinear.txt: -------------------------------------------------------------------------------- 1 | 0 0 1 0 2 | 0 0 0 1 3 | 0 0 0 0 4 | 0 0 0 0 5 | -------------------------------------------------------------------------------- /plotUtils/obstBelief.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/plotUtils/obstBelief.npy -------------------------------------------------------------------------------- /msg/linearMatrices.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64[49] Alinear 4 | float64[14] Blinear 5 | float64[7] Clinear -------------------------------------------------------------------------------- /qp_matrices/planningModelMatrices/Blinear.txt: -------------------------------------------------------------------------------- 1 | 0 0 2 | 0 0 3 | 0 0 4 | 0.20025 0.20025 5 | 0 0 6 | 0 0 7 | -0.54235 -0.54235 8 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/misc/lapacke.h: -------------------------------------------------------------------------------- 1 | Unexpected error. File contents could not be restored from local history during undo/redo. -------------------------------------------------------------------------------- /workspace/.metadata/.plugins/org.eclipse.core.runtime/.settings/org.eclipse.ui.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | showIntro=false 3 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/util/NonMPL2.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_MPL2_ONLY 2 | #error Including non-MPL2 code in EIGEN_MPL2_ONLY mode 3 | #endif 4 | -------------------------------------------------------------------------------- /src/workspace/.metadata/.plugins/org.eclipse.core.runtime/.settings/org.eclipse.ui.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | showIntro=false 3 | -------------------------------------------------------------------------------- /src/pyFun/__pycache__/MOMDP.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/src/pyFun/__pycache__/MOMDP.cpython-37.pyc -------------------------------------------------------------------------------- /src/pyFun/__pycache__/utils.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DrewSingletary/segway_sim/HEAD/src/pyFun/__pycache__/utils.cpython-37.pyc -------------------------------------------------------------------------------- /src/workspace/.metadata/.plugins/org.eclipse.core.runtime/.settings/org.eclipse.core.resources.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | version=1 3 | -------------------------------------------------------------------------------- /qp_matrices/tuning/Qerr.txt: -------------------------------------------------------------------------------- 1 | 0.0 0.0 0.0 0.0 0.0 0.0 0.0 2 | 100.0 100.0 100.0 100.0 100.0 100.0 100.0 3 | 1000.0 1000.0 1000.0 100.0 100.0 100.0 100.0 -------------------------------------------------------------------------------- /workspace/.metadata/.plugins/org.eclipse.core.runtime/.settings/com.digi.xctung.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | generalVerificationDate=1605656078722 3 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /msg/highLevelBelief.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 probMiss 4 | float64[40] bt 5 | float64[20] prob 6 | float64[2] targetPosDrone 7 | float64[2] targetPosSegway 8 | -------------------------------------------------------------------------------- /qp_matrices/planningModelMatrices/Alinear.txt: -------------------------------------------------------------------------------- 1 | 0 0 0 0 0 0 0 2 | 0 0 0 0 0 0 0 3 | 0 0 0 0 1 0 0 4 | 0 0 0 -2.3341 0 -2.6158 0 5 | 0 0 0 0 0 0 0 6 | 0 0 0 0 0 0 1 7 | 0 0 0 6.3216 0 18.331 0 -------------------------------------------------------------------------------- /msg/state.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 time 4 | float64 x 5 | float64 y 6 | float64 theta 7 | float64 v 8 | float64 thetaDot 9 | float64 psi 10 | float64 psiDot 11 | float64[7] stateVec -------------------------------------------------------------------------------- /include/segway_sim/sensor_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_NODE_H 2 | #define SENSOR_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/state.h" 6 | #include "segway_sim/sensor.h" 7 | #include "segway_sim/common.hpp" 8 | 9 | #endif -------------------------------------------------------------------------------- /msg/lowLevelLog.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 time 4 | float64[7] X 5 | float64[7] Xn 6 | float64[2] uMPC 7 | float64[2] uCBF 8 | float64[2] uTot 9 | float64 flagQP 10 | float64 V 11 | float64 h 12 | float64 QPtime 13 | -------------------------------------------------------------------------------- /msg/optSol.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 time 4 | float64[367] optimalSolution 5 | float64 solverFlag 6 | float64 solverTime 7 | float64 x 8 | float64 y 9 | float64[7] xCurr 10 | float64[7] x_IC 11 | float64 delay_ms 12 | float64 contPlan -------------------------------------------------------------------------------- /msg/goalSetAndState.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 x 4 | float64 y 5 | float64 xmin 6 | float64 xmax 7 | float64 ymin 8 | float64 ymax 9 | float64 highLevTime 10 | float64 term_xmin 11 | float64 term_xmax 12 | float64 term_ymin 13 | float64 term_ymax -------------------------------------------------------------------------------- /src/workspace/.metadata/.plugins/org.eclipse.core.runtime/.settings/com.digi.xctung.prefs: -------------------------------------------------------------------------------- 1 | addModulePortName=/dev/ttyUSB1 2 | eclipse.preferences.version=1 3 | generalRatingRemainingTime=281637 4 | generalSurveyRemainingTime=1494866 5 | generalVerificationDate=1606156610991 6 | -------------------------------------------------------------------------------- /launch/start_momdp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/README.md: -------------------------------------------------------------------------------- 1 | To run single agent in sim: 2 | 1) roslaunch segway_sim main.launch 3 | 2) rosservice call /segway_sim/integrator/ui "cmd: 1" 4 | 5 | To run multi-agent in sim: 6 | 1) roslaunch segway_sim mpcDrone.launch 7 | 2) roslaunch segway_sim mainMultiAgent.launch 8 | 3) rosservice call /segway_sim/integrator/ui "cmd: 1" 9 | -------------------------------------------------------------------------------- /msg/valFunCnst.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 x1 4 | float64 y1 5 | float64 c1 6 | float64 x2 7 | float64 y2 8 | float64 c2 9 | float64 x3 10 | float64 y3 11 | float64 c3 12 | float64 xmin 13 | float64 xmax 14 | float64 ymin 15 | float64 ymax 16 | float64 highLevTime 17 | float64 term_xmin 18 | float64 term_xmax 19 | float64 term_ymin 20 | float64 term_ymax 21 | -------------------------------------------------------------------------------- /include/segway_sim/controller_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_NODE_H 2 | #define CONTROLLER_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/common.hpp" 6 | #include "segway_sim/input.h" 7 | #include "segway_sim/state.h" 8 | #include "segway_sim/cmd.h" 9 | #include "visualization_msgs/Marker.h" 10 | 11 | enum class STATUS : uint8_t 12 | { 13 | FAILURE = 0, 14 | RUNNING = 1 15 | }; 16 | 17 | #endif -------------------------------------------------------------------------------- /msg/stateDrone.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | Header header 3 | float64 time 4 | float64 x 5 | float64 y 6 | float64 z 7 | float64 qw 8 | float64 qx 9 | float64 qy 10 | float64 qz 11 | float64 vbx 12 | float64 vby 13 | float64 vbz 14 | float64 omegax 15 | float64 omegay 16 | float64 omegaz 17 | float64 omega1 18 | float64 omega2 19 | float64 omega3 20 | float64 omega4 21 | float64 vx 22 | float64 vy 23 | float64 vz 24 | -------------------------------------------------------------------------------- /launch/highLevelDrone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/highLevelMulti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/highLevelMulti_hw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/manual_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/misc/lapacke_mangling.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_HEADER_INCLUDED 2 | #define LAPACK_HEADER_INCLUDED 3 | 4 | #ifndef LAPACK_GLOBAL 5 | #if defined(LAPACK_GLOBAL_PATTERN_LC) || defined(ADD_) 6 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 7 | #elif defined(LAPACK_GLOBAL_PATTERN_UC) || defined(UPPER) 8 | #define LAPACK_GLOBAL(lcname,UCNAME) UCNAME 9 | #elif defined(LAPACK_GLOBAL_PATTERN_MC) || defined(NOCHANGE) 10 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname 11 | #else 12 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 13 | #endif 14 | #endif 15 | 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /include/segway_sim/low_level_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LOW_LEVEL_NODE_H 2 | #define LOW_LEVEL_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/cmd.h" 6 | #include "segway_sim/input.h" 7 | #include "segway_sim/state.h" 8 | #include "segway_sim/common.hpp" 9 | #include "segway_sim/linearMatrices.h" 10 | #include "segway_sim/lowLevelLog.h" 11 | #include "segway_sim/optSol.h" 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #endif -------------------------------------------------------------------------------- /include/segway_sim/ekf.h: -------------------------------------------------------------------------------- 1 | #ifndef EKF_H 2 | #define EKF_H 3 | 4 | #include "dynamics_ekf.h" 5 | #include 6 | 7 | class EKF 8 | { 9 | public: 10 | // Methods 11 | EKF(const std::vector &Q, 12 | const std::vector &R); 13 | 14 | void init(const std::vector &x0, 15 | const std::vector &P0vec); 16 | 17 | void update(const std::vector &U, 18 | const std::vector &M, 19 | const double dt); 20 | 21 | // Attributes 22 | std::vector x_t_; 23 | std::vector P_t_; 24 | const std::vector Q_; 25 | const std::vector R_; 26 | uint32_t initialized = 0; 27 | }; 28 | 29 | #endif -------------------------------------------------------------------------------- /include/segway_sim/keyboard.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KEYBOARD_H 2 | #define KEYBOARD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | struct keyboard_state 11 | { 12 | signed short keys[KEY_CNT]; 13 | }; 14 | 15 | class cKeyboard 16 | { 17 | public: 18 | pthread_t thread; 19 | bool active; 20 | int keyboard_fd; 21 | input_event *keyboard_ev; 22 | keyboard_state *keyboard_st; 23 | char name[256]; 24 | 25 | protected: 26 | public: 27 | cKeyboard(const char* dev); 28 | ~cKeyboard(); 29 | static void* loop(void* obj); 30 | void readEv(); 31 | short getKeyState(short key); 32 | }; 33 | 34 | #endif -------------------------------------------------------------------------------- /include/segway_sim/manual_teleop_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MANUAL_TELEOP_NODE_H 2 | #define MANUAL_TELEOP_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/input.h" 6 | #include "segway_sim/state.h" 7 | #include "segway_sim/cmd.h" 8 | #include "segway_sim/common.hpp" 9 | #include "segway_sim/keyboard.hpp" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | enum class STATUS : uint8_t 18 | { 19 | FAILURE = 0, 20 | RUNNING = 1 21 | }; 22 | 23 | std::map moveBindings 24 | { 25 | {'w', { 1., 0.}}, 26 | {'s', {-1., 0.}}, 27 | {'a', { 0., 1.}}, 28 | {'d', { 0.,-1.}}, 29 | }; 30 | 31 | #endif -------------------------------------------------------------------------------- /launch/drone_hardware.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/functors/TernaryFunctors.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TERNARY_FUNCTORS_H 11 | #define EIGEN_TERNARY_FUNCTORS_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | //---------- associative ternary functors ---------- 18 | 19 | 20 | 21 | } // end namespace internal 22 | 23 | } // end namespace Eigen 24 | 25 | #endif // EIGEN_TERNARY_FUNCTORS_H 26 | -------------------------------------------------------------------------------- /include/Eigen_embedded/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(RegexUtils) 2 | test_escape_string_as_regex() 3 | 4 | file(GLOB Eigen_directory_files "*") 5 | 6 | escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 7 | 8 | foreach(f ${Eigen_directory_files}) 9 | if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src") 10 | list(APPEND Eigen_directory_files_to_install ${f}) 11 | endif() 12 | endforeach(f ${Eigen_directory_files}) 13 | 14 | install(FILES 15 | ${Eigen_directory_files_to_install} 16 | DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel 17 | ) 18 | 19 | install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | segway_sim 5 | 0.1.0 6 | The segway_sim package 7 | 8 | Drew 9 | 10 | MIT 11 | 12 | catkin 13 | roscpp 14 | pcl_conversions 15 | pcl_ros 16 | ambercortex_ros 17 | 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | visualization_msgs 22 | nav_msgs 23 | message_generation 24 | tf 25 | message_runtime 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /include/Eigen_embedded/StdList: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_STDLIST_MODULE_H 11 | #define EIGEN_STDLIST_MODULE_H 12 | 13 | #include "Core" 14 | #include 15 | 16 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 17 | 18 | #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) 19 | 20 | #else 21 | 22 | #include "src/StlSupport/StdList.h" 23 | 24 | #endif 25 | 26 | #endif // EIGEN_STDLIST_MODULE_H 27 | -------------------------------------------------------------------------------- /include/segway_sim/integrator_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INTEGRATOR_NODE_H 2 | #define INTEGRATOR_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "std_msgs/String.h" 6 | #include "std_msgs/Float64MultiArray.h" 7 | #include "segway_sim/input.h" 8 | #include "segway_sim/state.h" 9 | #include "segway_sim/ui.h" 10 | #include "segway_sim/common.hpp" 11 | #include "segway_sim/dynamics.hpp" 12 | 13 | #include 14 | 15 | #include 16 | 17 | using namespace boost::numeric::odeint; 18 | typedef std::vector state_t; 19 | typedef runge_kutta4 stepper_t; 20 | 21 | enum class STATUS : uint8_t 22 | { 23 | FAILURE = 0, 24 | STOPPED = 1, 25 | RUNNING = 2 26 | }; 27 | 28 | enum class CMD : uint8_t 29 | { 30 | STOP = 0, 31 | START = 1, 32 | PAUSE = 2, 33 | REPOSITION = 3, 34 | RESET = 4 35 | }; 36 | 37 | #endif -------------------------------------------------------------------------------- /include/Eigen_embedded/StdDeque: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDDEQUE_MODULE_H 12 | #define EIGEN_STDDEQUE_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdDeque.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDDEQUE_MODULE_H 28 | -------------------------------------------------------------------------------- /include/Eigen_embedded/StdVector: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDVECTOR_MODULE_H 12 | #define EIGEN_STDVECTOR_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdVector.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDVECTOR_MODULE_H 28 | -------------------------------------------------------------------------------- /launch/mpcDrone.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED 2 | #undef EIGEN_WARNINGS_DISABLED 3 | 4 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 5 | #ifdef _MSC_VER 6 | #pragma warning( pop ) 7 | #elif defined __INTEL_COMPILER 8 | #pragma warning pop 9 | #elif defined __clang__ 10 | #pragma clang diagnostic pop 11 | #elif defined __GNUC__ && __GNUC__>=6 12 | #pragma GCC diagnostic pop 13 | #endif 14 | 15 | #if defined __NVCC__ 16 | // Don't reenable the diagnostic messages, as it turns out these messages need 17 | // to be disabled at the point of the template instantiation (i.e the user code) 18 | // otherwise they'll be triggered by nvcc. 19 | // #pragma diag_default code_is_unreachable 20 | // #pragma diag_default initialization_not_reachable 21 | // #pragma diag_default 2651 22 | // #pragma diag_default 2653 23 | #endif 24 | 25 | #endif 26 | 27 | #endif // EIGEN_WARNINGS_DISABLED 28 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Householder: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 9 | #define EIGEN_HOUSEHOLDER_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Householder_Module Householder module 16 | * This module provides Householder transformations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/Householder/Householder.h" 24 | #include "src/Householder/HouseholderSequence.h" 25 | #include "src/Householder/BlockHouseholder.h" 26 | 27 | #include "src/Core/util/ReenableStupidWarnings.h" 28 | 29 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 30 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 31 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Sparse: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSE_MODULE_H 9 | #define EIGEN_SPARSE_MODULE_H 10 | 11 | /** \defgroup Sparse_Module Sparse meta-module 12 | * 13 | * Meta-module including all related modules: 14 | * - \ref SparseCore_Module 15 | * - \ref OrderingMethods_Module 16 | * - \ref SparseCholesky_Module 17 | * - \ref SparseLU_Module 18 | * - \ref SparseQR_Module 19 | * - \ref IterativeLinearSolvers_Module 20 | * 21 | \code 22 | #include 23 | \endcode 24 | */ 25 | 26 | #include "SparseCore" 27 | #include "OrderingMethods" 28 | #ifndef EIGEN_MPL2_ONLY 29 | #include "SparseCholesky" 30 | #endif 31 | #include "SparseLU" 32 | #include "SparseQR" 33 | #include "IterativeLinearSolvers" 34 | 35 | #endif // EIGEN_SPARSE_MODULE_H 36 | 37 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Jacobi: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_JACOBI_MODULE_H 9 | #define EIGEN_JACOBI_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Jacobi_Module Jacobi module 16 | * This module provides Jacobi and Givens rotations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | * 22 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 23 | * - MatrixBase::applyOnTheLeft() 24 | * - MatrixBase::applyOnTheRight(). 25 | */ 26 | 27 | #include "src/Jacobi/Jacobi.h" 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_JACOBI_MODULE_H 32 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 33 | 34 | -------------------------------------------------------------------------------- /launch/rvizMultiAgent.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/DiagonalProduct.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2007-2009 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_DIAGONALPRODUCT_H 12 | #define EIGEN_DIAGONALPRODUCT_H 13 | 14 | namespace Eigen { 15 | 16 | /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. 17 | */ 18 | template 19 | template 20 | inline const Product 21 | MatrixBase::operator*(const DiagonalBase &a_diagonal) const 22 | { 23 | return Product(derived(),a_diagonal.derived()); 24 | } 25 | 26 | } // end namespace Eigen 27 | 28 | #endif // EIGEN_DIAGONALPRODUCT_H 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 DrewSingletary 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /include/Eigen_embedded/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QTMALLOC_MODULE_H 9 | #define EIGEN_QTMALLOC_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | void *qMalloc(std::size_t size) 18 | { 19 | return Eigen::internal::aligned_malloc(size); 20 | } 21 | 22 | void qFree(void *ptr) 23 | { 24 | Eigen::internal::aligned_free(ptr); 25 | } 26 | 27 | void *qRealloc(void *ptr, std::size_t size) 28 | { 29 | void* newPtr = Eigen::internal::aligned_malloc(size); 30 | std::memcpy(newPtr, ptr, size); 31 | Eigen::internal::aligned_free(ptr); 32 | return newPtr; 33 | } 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | 39 | #endif // EIGEN_QTMALLOC_MODULE_H 40 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 41 | -------------------------------------------------------------------------------- /include/Eigen_embedded/MetisSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_METISSUPPORT_MODULE_H 9 | #define EIGEN_METISSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup MetisSupport_Module MetisSupport module 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 27 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 28 | */ 29 | 30 | 31 | #include "src/MetisSupport/MetisSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_METISSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /src/keyboard.cpp: -------------------------------------------------------------------------------- 1 | #include "segway_sim/keyboard.hpp" 2 | 3 | cKeyboard::cKeyboard(const char* dev) 4 | { 5 | active = false; 6 | keyboard_fd = 0; 7 | keyboard_ev = new input_event(); 8 | keyboard_st = new keyboard_state(); 9 | keyboard_fd = open(dev, O_RDONLY); 10 | if (keyboard_fd > 0) { 11 | ioctl(keyboard_fd, EVIOCGNAME(256), name); 12 | active = true; 13 | pthread_create(&thread, 0, &cKeyboard::loop, this); 14 | } 15 | } 16 | 17 | cKeyboard::~cKeyboard() 18 | { 19 | if (keyboard_fd > 0) { 20 | active = false; 21 | pthread_cancel(thread); 22 | close(keyboard_fd); 23 | } 24 | delete keyboard_st; 25 | delete keyboard_ev; 26 | keyboard_fd = 0; 27 | } 28 | 29 | void* cKeyboard::loop(void *obj) 30 | { 31 | while (reinterpret_cast(obj)->active) reinterpret_cast(obj)->readEv(); 32 | } 33 | 34 | void cKeyboard::readEv() 35 | { 36 | int bytes = read(keyboard_fd, keyboard_ev, sizeof(*keyboard_ev)); 37 | if (bytes > 0) { 38 | if (keyboard_ev->type & EV_KEY) { 39 | keyboard_st->keys[keyboard_ev->code] = keyboard_ev->value; 40 | } 41 | } 42 | } 43 | 44 | short cKeyboard::getKeyState(short key) 45 | { 46 | return keyboard_st->keys[key]; 47 | } -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseCore/SparseFuzzy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_FUZZY_H 11 | #define EIGEN_SPARSE_FUZZY_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const 18 | { 19 | const typename internal::nested_eval::type actualA(derived()); 20 | typename internal::conditional::type, 22 | const PlainObject>::type actualB(other.derived()); 23 | 24 | return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); 25 | } 26 | 27 | } // end namespace Eigen 28 | 29 | #endif // EIGEN_SPARSE_FUZZY_H 30 | -------------------------------------------------------------------------------- /include/Eigen_embedded/PardisoSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PARDISOSUPPORT_MODULE_H 9 | #define EIGEN_PARDISOSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup PardisoSupport_Module PardisoSupport module 19 | * 20 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. 27 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 28 | * 29 | */ 30 | 31 | #include "src/PardisoSupport/PardisoSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /workspace/.metadata/.log: -------------------------------------------------------------------------------- 1 | !SESSION 2020-11-17 15:34:36.398 ----------------------------------------------- 2 | eclipse.buildId=unknown 3 | java.version=1.8.0_131 4 | java.vendor=Oracle Corporation 5 | BootLoader constants: OS=linux, ARCH=x86_64, WS=gtk, NL=en_US 6 | Framework arguments: -clearPersistedState 7 | Command-line arguments: -os linux -ws gtk -arch x86_64 -clearPersistedState 8 | 9 | !ENTRY org.eclipse.ui 2 2 2020-11-17 15:34:38.485 10 | !MESSAGE Perspective with name 'Configuration perspective' and id 'com.digi.xctung.perspectives.configurationPerspective' has been made into a local copy 11 | 12 | !ENTRY org.eclipse.ui 2 2 2020-11-17 15:34:38.513 13 | !MESSAGE Invalid preference category path: org.eclipse.equinox.internal.p2.ui.sdk.ProvisioningPreferencePage (bundle: org.eclipse.equinox.p2.ui.sdk.scheduler, page: org.eclipse.equinox.internal.p2.ui.sdk.scheduler.AutomaticUpdatesPreferencePage) 14 | 15 | !ENTRY org.eclipse.equinox.p2.repository 2 0 2020-11-17 15:34:44.457 16 | !MESSAGE Server returned lastModified <= 0 for ftp://ftp1.digi.com/support/XCTU-NG/java_18/repository/content.xml 17 | 18 | !ENTRY org.eclipse.ui 2 2 2020-11-17 15:34:58.972 19 | !MESSAGE Perspective with name 'Consoles perspective' and id 'com.digi.xctung.perspectives.consolesPerspective' has been made into a local copy 20 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SPQRSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 9 | #define EIGEN_SPQRSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SuiteSparseQR.hpp" 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup SPQRSupport_Module SuiteSparseQR module 19 | * 20 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). 27 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 28 | * 29 | */ 30 | 31 | #include "src/CholmodSupport/CholmodSupport.h" 32 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SparseQR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSEQR_MODULE_H 9 | #define EIGEN_SPARSEQR_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup SparseQR_Module SparseQR module 16 | * \brief Provides QR decomposition for sparse matrices 17 | * 18 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 19 | * The columns of the input matrix should be reordered to limit the fill-in during the 20 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 21 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 22 | * of built-in and external ordering methods. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | * 28 | * 29 | */ 30 | 31 | #include "OrderingMethods" 32 | #include "src/SparseCore/SparseColEtree.h" 33 | #include "src/SparseQR/SparseQR.h" 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Cholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLESKY_MODULE_H 9 | #define EIGEN_CHOLESKY_MODULE_H 10 | 11 | #include "Core" 12 | #include "Jacobi" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** \defgroup Cholesky_Module Cholesky module 17 | * 18 | * 19 | * 20 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 21 | * Those decompositions are also accessible via the following methods: 22 | * - MatrixBase::llt() 23 | * - MatrixBase::ldlt() 24 | * - SelfAdjointView::llt() 25 | * - SelfAdjointView::ldlt() 26 | * 27 | * \code 28 | * #include 29 | * \endcode 30 | */ 31 | 32 | #include "src/Cholesky/LLT.h" 33 | #include "src/Cholesky/LDLT.h" 34 | #ifdef EIGEN_USE_LAPACKE 35 | #ifdef EIGEN_USE_MKL 36 | #include "mkl_lapacke.h" 37 | #else 38 | #include "src/misc/lapacke.h" 39 | #endif 40 | #include "src/Cholesky/LLT_LAPACKE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_CHOLESKY_MODULE_H 46 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 47 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/AVX/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_AVX_H 11 | #define EIGEN_TYPE_CASTING_AVX_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // For now we use SSE to handle integers, so we can't use AVX instructions to cast 18 | // from int to float 19 | template <> 20 | struct type_casting_traits { 21 | enum { 22 | VectorizedCast = 0, 23 | SrcCoeffRatio = 1, 24 | TgtCoeffRatio = 1 25 | }; 26 | }; 27 | 28 | template <> 29 | struct type_casting_traits { 30 | enum { 31 | VectorizedCast = 0, 32 | SrcCoeffRatio = 1, 33 | TgtCoeffRatio = 1 34 | }; 35 | }; 36 | 37 | 38 | 39 | template<> EIGEN_STRONG_INLINE Packet8i pcast(const Packet8f& a) { 40 | return _mm256_cvtps_epi32(a); 41 | } 42 | 43 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8i& a) { 44 | return _mm256_cvtepi32_ps(a); 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_TYPE_CASTING_AVX_H 52 | -------------------------------------------------------------------------------- /plotUtils/plotGif.py: -------------------------------------------------------------------------------- 1 | import pdb 2 | import matplotlib.pyplot as plt 3 | import numpy as np 4 | from matplotlib.patches import Rectangle 5 | from matplotlib.animation import FuncAnimation 6 | import scipy.io as sio 7 | from tempfile import TemporaryFile 8 | 9 | 10 | 11 | def saveGit(name, xaxis, variableAnimate, color, labels, yLimits): 12 | fig = plt.figure() 13 | fig.set_tight_layout(True) 14 | ax = plt.axes() 15 | lineList = [] 16 | for i in range(0, len(variableAnimate)): 17 | line, = ax.plot([], [], color[i], label=labels[i],zorder=1) 18 | lineList.append(line) 19 | plt.legend() 20 | 21 | def update(i): 22 | for j in range(0, len(variableAnimate)): 23 | lineList[j].set_data(xaxis[0:i], variableAnimate[j][0:i]) 24 | ax.set(xlim=(0, i+1), ylim=yLimits) 25 | dataPoints = variableAnimate[0].shape[0] 26 | 27 | anim = FuncAnimation(fig, update, frames=np.arange(0, dataPoints+1), interval=100) 28 | anim.save(name+'.gif', dpi=80, writer='imagemagick') 29 | 30 | def main(): 31 | # saveGit('prob', time_belief, [np.array(probMiss), probObstArray[:,0], probObstArray[:,1], probObstArray[:,2], probObstArray[:,3]], ['-k','-ob','--sb','-og','--sg'],['Mission', 'R1', 'R2', 'G1', 'G2'], (-0.1, 1.3)) 32 | with open('test.npy', 'rb') as f: 33 | a = np.load(f) 34 | b = np.load(f) 35 | c = np.load(f) 36 | 37 | pdb.set_trace() 38 | T = 100 39 | saveGit('compTime', a[0:T], [np.array(b[0:T]), np.array(c[0:T])], ['-k','-r'],['Segway MPC', 'Wheeled bot MPC'], (-0.05, 0.5)) 40 | 41 | main() 42 | 43 | ## Read and plot STATE 44 | -------------------------------------------------------------------------------- /include/Eigen_embedded/QR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QR_MODULE_H 9 | #define EIGEN_QR_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | 19 | /** \defgroup QR_Module QR module 20 | * 21 | * 22 | * 23 | * This module provides various QR decompositions 24 | * This module also provides some MatrixBase methods, including: 25 | * - MatrixBase::householderQr() 26 | * - MatrixBase::colPivHouseholderQr() 27 | * - MatrixBase::fullPivHouseholderQr() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/QR/HouseholderQR.h" 35 | #include "src/QR/FullPivHouseholderQR.h" 36 | #include "src/QR/ColPivHouseholderQR.h" 37 | #include "src/QR/CompleteOrthogonalDecomposition.h" 38 | #ifdef EIGEN_USE_LAPACKE 39 | #ifdef EIGEN_USE_MKL 40 | #include "mkl_lapacke.h" 41 | #else 42 | #include "src/misc/lapacke.h" 43 | #endif 44 | #include "src/QR/HouseholderQR_LAPACKE.h" 45 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_QR_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /include/Eigen_embedded/UmfPackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 9 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup UmfPackSupport_Module UmfPackSupport module 21 | * 22 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 23 | * It provides the following factorization class: 24 | * - class UmfPackLU: a multifrontal sequential LU factorization. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. 31 | * The dependencies depend on how umfpack has been compiled. 32 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/UmfPackSupport/UmfPackSupport.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 41 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SparseCholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2013 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSECHOLESKY_MODULE_H 11 | #define EIGEN_SPARSECHOLESKY_MODULE_H 12 | 13 | #include "SparseCore" 14 | #include "OrderingMethods" 15 | 16 | #include "src/Core/util/DisableStupidWarnings.h" 17 | 18 | /** 19 | * \defgroup SparseCholesky_Module SparseCholesky module 20 | * 21 | * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. 22 | * Those decompositions are accessible via the following classes: 23 | * - SimplicialLLt, 24 | * - SimplicialLDLt 25 | * 26 | * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #ifdef EIGEN_MPL2_ONLY 34 | #error The SparseCholesky module has nothing to offer in MPL2 only mode 35 | #endif 36 | 37 | #include "src/SparseCholesky/SimplicialCholesky.h" 38 | 39 | #ifndef EIGEN_MPL2_ONLY 40 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 46 | -------------------------------------------------------------------------------- /src/pyFun/pyMOMDP.yml: -------------------------------------------------------------------------------- 1 | name: pyMOMDP 2 | channels: 3 | - conda-forge 4 | - bioconda 5 | - defaults 6 | dependencies: 7 | - ca-certificates=2020.6.20=hecda079_0 8 | - certifi=2020.6.20=py38h32f6830_0 9 | - cycler=0.10.0=py_2 10 | - freetype=2.10.2=h8da9a1a_0 11 | - jpeg=9d=h0b31af3_0 12 | - kiwisolver=1.2.0=py38ha0d09dd_0 13 | - lcms2=2.11=h174193d_0 14 | - libblas=3.8.0=16_mkl 15 | - libcblas=3.8.0=16_mkl 16 | - libcxx=10.0.1=h5f48129_0 17 | - libffi=3.2.1=hb1e8313_1007 18 | - libgfortran=4.0.0=3 19 | - libgfortran4=7.5.0=h1565451_3 20 | - liblapack=3.8.0=16_mkl 21 | - libopenblas=0.3.10=openmp_h63d9170_4 22 | - libpng=1.6.37=hb0a8c7a_2 23 | - libtiff=4.1.0=h2ae36a8_6 24 | - libwebp-base=1.1.0=h0b31af3_3 25 | - llvm-openmp=10.0.1=h28b9765_0 26 | - lz4-c=1.9.2=hb1e8313_3 27 | - matplotlib=3.3.1=1 28 | - matplotlib-base=3.3.1=py38haed9462_1 29 | - mkl=2020.2=260 30 | - ncurses=6.2=hb1e8313_1 31 | - numpy=1.19.1=py38h8ccc501_2 32 | - olefile=0.46=py_0 33 | - openssl=1.1.1g=haf1e3a3_1 34 | - pillow=7.2.0=py38h83dc5e5_1 35 | - pip=20.2.3=py_0 36 | - pyparsing=2.4.7=pyh9f0ad1d_0 37 | - python=3.8.5=hfc71d35_7_cpython 38 | - python-dateutil=2.8.1=py_0 39 | - python_abi=3.8=1_cp38 40 | - readline=8.0=h0678c8f_2 41 | - scipy=1.5.2=py38h1402333_0 42 | - setuptools=49.6.0=py38h32f6830_0 43 | - six=1.15.0=pyh9f0ad1d_0 44 | - sqlite=3.33.0=h960bd1c_0 45 | - tk=8.6.10=hb0a8c7a_0 46 | - tornado=6.0.4=py38h64e0658_1 47 | - wheel=0.35.1=pyh9f0ad1d_0 48 | - xz=5.2.5=haf1e3a3_1 49 | - zlib=1.2.11=h7795811_1009 50 | - zstd=1.4.5=h289c70a_2 51 | prefix: /opt/anaconda3/envs/pyMOMDP 52 | -------------------------------------------------------------------------------- /include/Eigen_embedded/LU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_LU_MODULE_H 9 | #define EIGEN_LU_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup LU_Module LU module 16 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 17 | * This module defines the following MatrixBase methods: 18 | * - MatrixBase::inverse() 19 | * - MatrixBase::determinant() 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/misc/Kernel.h" 27 | #include "src/misc/Image.h" 28 | #include "src/LU/FullPivLU.h" 29 | #include "src/LU/PartialPivLU.h" 30 | #ifdef EIGEN_USE_LAPACKE 31 | #ifdef EIGEN_USE_MKL 32 | #include "mkl_lapacke.h" 33 | #else 34 | #include "src/misc/lapacke.h" 35 | #endif 36 | #include "src/LU/PartialPivLU_LAPACKE.h" 37 | #endif 38 | #include "src/LU/Determinant.h" 39 | #include "src/LU/InverseImpl.h" 40 | 41 | // Use the SSE optimized version whenever possible. At the moment the 42 | // SSE version doesn't compile when AVX is enabled 43 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 44 | #include "src/LU/arch/Inverse_SSE.h" 45 | #endif 46 | 47 | // #include "src/Core/util/ReenableStupidWarnings.h" 48 | 49 | #endif // EIGEN_LU_MODULE_H 50 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 51 | -------------------------------------------------------------------------------- /include/segway_sim/mpc_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MPC_NODE_H 2 | #define MPC_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/cmd.h" 6 | #include "segway_sim/input.h" 7 | #include "segway_sim/state.h" 8 | #include "segway_sim/valFunCnst.h" 9 | #include "segway_sim/optSol.h" 10 | #include "segway_sim/goalSetAndState.h" 11 | #include "segway_sim/linearMatrices.h" 12 | #include "segway_sim/common.hpp" 13 | #include "ambercortex_ros/state.h" 14 | #include "ambercortex_ros/cmd.h" 15 | #include "sensor_msgs/Joy.h" 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #define MSG_MODE 'M' 26 | 27 | #define MODE_INIT 10 28 | #define MODE_RUN 20 29 | #define MODE_BACKUP 30 30 | #define MSG_CTRL 'K' 31 | 32 | static const uint32_t nx = 7; // Number of system state 33 | static const uint32_t nu = 2; // Number of inputs 34 | static const uint32_t N = 40; // Horizon length 35 | static const uint32_t printLevel = 0; // 0 = no printing, 1 = only x_t, 2 = minimal, 3 = minimal + initialization, 4 = all 36 | 37 | union num32_t { 38 | int32_t i; 39 | uint32_t ui; 40 | float f; 41 | uint8_t c[4]; 42 | }; 43 | 44 | void add_float_to_vec(std::vector & vec, float f) { 45 | // add four bits to the end of a std::vector representing a float 46 | num32_t n32; 47 | n32.f = f; 48 | vec.push_back(n32.c[0]); 49 | vec.push_back(n32.c[1]); 50 | vec.push_back(n32.c[2]); 51 | vec.push_back(n32.c[3]); 52 | } 53 | 54 | uint8_t compute_vec_chksum(const std::vector & vec) { 55 | // compute checksum of a std::vector 56 | uint8_t chksum = 0; 57 | for (auto it = vec.cbegin(); it != vec.cend(); ++it) 58 | chksum += *it; 59 | return chksum; 60 | } 61 | 62 | #endif -------------------------------------------------------------------------------- /include/Eigen_embedded/SVD: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SVD_MODULE_H 9 | #define EIGEN_SVD_MODULE_H 10 | 11 | #include "QR" 12 | #include "Householder" 13 | #include "Jacobi" 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | /** \defgroup SVD_Module SVD module 18 | * 19 | * 20 | * 21 | * This module provides SVD decomposition for matrices (both real and complex). 22 | * Two decomposition algorithms are provided: 23 | * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. 24 | * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. 25 | * These decompositions are accessible via the respective classes and following MatrixBase methods: 26 | * - MatrixBase::jacobiSvd() 27 | * - MatrixBase::bdcSvd() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/misc/RealSvd2x2.h" 35 | #include "src/SVD/UpperBidiagonalization.h" 36 | #include "src/SVD/SVDBase.h" 37 | #include "src/SVD/JacobiSVD.h" 38 | #include "src/SVD/BDCSVD.h" 39 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 40 | #ifdef EIGEN_USE_MKL 41 | #include "mkl_lapacke.h" 42 | #else 43 | #include "src/misc/lapacke.h" 44 | #endif 45 | #include "src/SVD/JacobiSVD_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_SVD_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /include/Eigen_embedded/PaStiXSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 9 | #define EIGEN_PASTIXSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | #ifdef complex 21 | #undef complex 22 | #endif 23 | 24 | /** \ingroup Support_modules 25 | * \defgroup PaStiXSupport_Module PaStiXSupport module 26 | * 27 | * This module provides an interface to the PaSTiX library. 28 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 29 | * It provides the two following main factorization classes: 30 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 31 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 32 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | * 38 | * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. 39 | * The dependencies depend on how PaSTiX has been compiled. 40 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 41 | * 42 | */ 43 | 44 | #include "src/PaStiXSupport/PaStiXSupport.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 49 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SparseLU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // Copyright (C) 2012 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSELU_MODULE_H 12 | #define EIGEN_SPARSELU_MODULE_H 13 | 14 | #include "SparseCore" 15 | 16 | /** 17 | * \defgroup SparseLU_Module SparseLU module 18 | * This module defines a supernodal factorization of general sparse matrices. 19 | * The code is fully optimized for supernode-panel updates with specialized kernels. 20 | * Please, see the documentation of the SparseLU class for more details. 21 | */ 22 | 23 | // Ordering interface 24 | #include "OrderingMethods" 25 | 26 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 27 | 28 | #include "src/SparseLU/SparseLU_Structs.h" 29 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 30 | #include "src/SparseLU/SparseLUImpl.h" 31 | #include "src/SparseCore/SparseColEtree.h" 32 | #include "src/SparseLU/SparseLU_Memory.h" 33 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 34 | #include "src/SparseLU/SparseLU_relax_snode.h" 35 | #include "src/SparseLU/SparseLU_pivotL.h" 36 | #include "src/SparseLU/SparseLU_panel_dfs.h" 37 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 38 | #include "src/SparseLU/SparseLU_panel_bmod.h" 39 | #include "src/SparseLU/SparseLU_column_dfs.h" 40 | #include "src/SparseLU/SparseLU_column_bmod.h" 41 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 42 | #include "src/SparseLU/SparseLU_pruneL.h" 43 | #include "src/SparseLU/SparseLU_Utils.h" 44 | #include "src/SparseLU/SparseLU.h" 45 | 46 | #endif // EIGEN_SPARSELU_MODULE_H 47 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/SelfCwiseBinaryOp.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SELFCWISEBINARYOP_H 11 | #define EIGEN_SELFCWISEBINARYOP_H 12 | 13 | namespace Eigen { 14 | 15 | // TODO generalize the scalar type of 'other' 16 | 17 | template 18 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(const Scalar& other) 19 | { 20 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 21 | return derived(); 22 | } 23 | 24 | template 25 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 26 | { 27 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 28 | return derived(); 29 | } 30 | 31 | template 32 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 33 | { 34 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 35 | return derived(); 36 | } 37 | 38 | template 39 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 40 | { 41 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 42 | return derived(); 43 | } 44 | 45 | } // end namespace Eigen 46 | 47 | #endif // EIGEN_SELFCWISEBINARYOP_H 48 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseCore/SparseRedux.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSEREDUX_H 11 | #define EIGEN_SPARSEREDUX_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | typename internal::traits::Scalar 17 | SparseMatrixBase::sum() const 18 | { 19 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 20 | Scalar res(0); 21 | internal::evaluator thisEval(derived()); 22 | for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) 24 | res += iter.value(); 25 | return res; 26 | } 27 | 28 | template 29 | typename internal::traits >::Scalar 30 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 31 | { 32 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 33 | if(this->isCompressed()) 34 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 35 | else 36 | return Base::sum(); 37 | } 38 | 39 | template 40 | typename internal::traits >::Scalar 41 | SparseVector<_Scalar,_Options,_Index>::sum() const 42 | { 43 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 44 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 45 | } 46 | 47 | } // end namespace Eigen 48 | 49 | #endif // EIGEN_SPARSEREDUX_H 50 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/Default/Settings.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | 12 | /* All the parameters defined in this file can be specialized in the 13 | * architecture specific files, and/or by the user. 14 | * More to come... */ 15 | 16 | #ifndef EIGEN_DEFAULT_SETTINGS_H 17 | #define EIGEN_DEFAULT_SETTINGS_H 18 | 19 | /** Defines the maximal loop size to enable meta unrolling of loops. 20 | * Note that the value here is expressed in Eigen's own notion of "number of FLOPS", 21 | * it does not correspond to the number of iterations or the number of instructions 22 | */ 23 | #ifndef EIGEN_UNROLLING_LIMIT 24 | #define EIGEN_UNROLLING_LIMIT 100 25 | #endif 26 | 27 | /** Defines the threshold between a "small" and a "large" matrix. 28 | * This threshold is mainly used to select the proper product implementation. 29 | */ 30 | #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 31 | #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8 32 | #endif 33 | 34 | /** Defines the maximal width of the blocks used in the triangular product and solver 35 | * for vectors (level 2 blas xTRMV and xTRSV). The default is 8. 36 | */ 37 | #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 38 | #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8 39 | #endif 40 | 41 | 42 | /** Defines the default number of registers available for that architecture. 43 | * Currently it must be 8 or 16. Other values will fail. 44 | */ 45 | #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 46 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8 47 | #endif 48 | 49 | #endif // EIGEN_DEFAULT_SETTINGS_H 50 | -------------------------------------------------------------------------------- /include/segway_sim/mpc_drone_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MPC_NODE_H 2 | #define MPC_NODE_H 3 | 4 | #include "ros/ros.h" 5 | #include "segway_sim/cmd.h" 6 | #include "segway_sim/input.h" 7 | #include "segway_sim/state.h" 8 | #include "segway_sim/valFunCnst.h" 9 | #include "segway_sim/optSol.h" 10 | #include "segway_sim/goalSetAndState.h" 11 | #include "segway_sim/linearMatrices.h" 12 | #include "segway_sim/common.hpp" 13 | #include "segway_sim/cmdDrone.h" 14 | #include "segway_sim/stateDrone.h" 15 | #include "segway_sim/cmdDrone.h" 16 | #include "ambercortex_ros/state.h" 17 | #include "ambercortex_ros/cmd.h" 18 | #include "ambercortex_ros/ctrl_info.h" 19 | #include "sensor_msgs/Joy.h" 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define MSG_MODE 'M' 30 | 31 | #define MODE_INIT 10 32 | #define MODE_RUN 20 33 | #define MODE_BACKUP 30 34 | #define MSG_CTRL 'K' 35 | 36 | static const int nx = 4; // Number of system state 37 | static const int nu = 2; // Number of inputs 38 | static const int N = 20; // Horizon length 39 | static const uint32_t printLevel = 0; // 0 = no printing, 1 = only x_t, 2 = minimal, 3 = minimal + initialization, 4 = all 40 | 41 | union num32_t { 42 | int32_t i; 43 | uint32_t ui; 44 | float f; 45 | uint8_t c[4]; 46 | }; 47 | 48 | void add_float_to_vec(std::vector & vec, float f) { 49 | // add four bits to the end of a std::vector representing a float 50 | num32_t n32; 51 | n32.f = f; 52 | vec.push_back(n32.c[0]); 53 | vec.push_back(n32.c[1]); 54 | vec.push_back(n32.c[2]); 55 | vec.push_back(n32.c[3]); 56 | } 57 | 58 | uint8_t compute_vec_chksum(const std::vector & vec) { 59 | // compute checksum of a std::vector 60 | uint8_t chksum = 0; 61 | for (auto it = vec.cbegin(); it != vec.cend(); ++it) 62 | chksum += *it; 63 | return chksum; 64 | } 65 | 66 | #endif -------------------------------------------------------------------------------- /include/segway_sim/CyberTimer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _CUSTOM_TIMER_H_ 2 | #define _CUSTOM_TIMER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | template 9 | class CyberTimer 10 | { 11 | static_assert(averageLen>0, "Averaging length must be strictly positive"); 12 | 13 | public: 14 | CyberTimer(void): 15 | i_(0), 16 | iterCount_(0), 17 | running_(false), 18 | dt_(0.0) 19 | {}; 20 | 21 | ~CyberTimer() 22 | {}; 23 | 24 | void tic(void) 25 | { 26 | running_ = true; 27 | t_start_ = std::chrono::high_resolution_clock::now(); 28 | }; 29 | 30 | double toc(void) 31 | { 32 | if(running_) 33 | { 34 | running_ = false; 35 | const auto t_stop = std::chrono::high_resolution_clock::now(); 36 | const auto duration = std::chrono::duration_cast(t_stop - t_start_); 37 | dt_ = static_cast(duration.count())*1.0e-9; 38 | 39 | buffer_[i_] = dt_; 40 | i_++; 41 | if(i_>=averageLen) 42 | i_=0; 43 | iterCount_++; 44 | } 45 | 46 | return dt_; 47 | }; 48 | 49 | double getAverage(void) const 50 | { 51 | if(iterCount_==0) 52 | return -1.0; 53 | 54 | if(iterCount_ t_start_; 87 | std::array buffer_; 88 | }; 89 | 90 | 91 | #endif -------------------------------------------------------------------------------- /include/Eigen_embedded/src/misc/RealSvd2x2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Benoit Jacob 5 | // Copyright (C) 2013-2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_REALSVD2X2_H 12 | #define EIGEN_REALSVD2X2_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, 20 | JacobiRotation *j_left, 21 | JacobiRotation *j_right) 22 | { 23 | using std::sqrt; 24 | using std::abs; 25 | Matrix m; 26 | m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), 27 | numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); 28 | JacobiRotation rot1; 29 | RealScalar t = m.coeff(0,0) + m.coeff(1,1); 30 | RealScalar d = m.coeff(1,0) - m.coeff(0,1); 31 | 32 | if(abs(d) < (std::numeric_limits::min)()) 33 | { 34 | rot1.s() = RealScalar(0); 35 | rot1.c() = RealScalar(1); 36 | } 37 | else 38 | { 39 | // If d!=0, then t/d cannot overflow because the magnitude of the 40 | // entries forming d are not too small compared to the ones forming t. 41 | RealScalar u = t / d; 42 | RealScalar tmp = sqrt(RealScalar(1) + numext::abs2(u)); 43 | rot1.s() = RealScalar(1) / tmp; 44 | rot1.c() = u / tmp; 45 | } 46 | m.applyOnTheLeft(0,1,rot1); 47 | j_right->makeJacobi(m,0,1); 48 | *j_left = rot1 * j_right->transpose(); 49 | } 50 | 51 | } // end namespace internal 52 | 53 | } // end namespace Eigen 54 | 55 | #endif // EIGEN_REALSVD2X2_H 56 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Eigenvalues: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_EIGENVALUES_MODULE_H 9 | #define EIGEN_EIGENVALUES_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | #include "LU" 19 | #include "Geometry" 20 | 21 | /** \defgroup Eigenvalues_Module Eigenvalues module 22 | * 23 | * 24 | * 25 | * This module mainly provides various eigenvalue solvers. 26 | * This module also provides some MatrixBase methods, including: 27 | * - MatrixBase::eigenvalues(), 28 | * - MatrixBase::operatorNorm() 29 | * 30 | * \code 31 | * #include 32 | * \endcode 33 | */ 34 | 35 | #include "src/misc/RealSvd2x2.h" 36 | #include "src/Eigenvalues/Tridiagonalization.h" 37 | #include "src/Eigenvalues/RealSchur.h" 38 | #include "src/Eigenvalues/EigenSolver.h" 39 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 40 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 41 | #include "src/Eigenvalues/HessenbergDecomposition.h" 42 | #include "src/Eigenvalues/ComplexSchur.h" 43 | #include "src/Eigenvalues/ComplexEigenSolver.h" 44 | #include "src/Eigenvalues/RealQZ.h" 45 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 46 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 47 | #ifdef EIGEN_USE_LAPACKE 48 | #ifdef EIGEN_USE_MKL 49 | #include "mkl_lapacke.h" 50 | #else 51 | #include "src/misc/lapacke.h" 52 | #endif 53 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 54 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 55 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_EIGENVALUES_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | -------------------------------------------------------------------------------- /include/Eigen_embedded/CholmodSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 9 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup CholmodSupport_Module CholmodSupport module 21 | * 22 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 23 | * It provides the two following main factorization classes: 24 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 25 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 26 | * 27 | * For the sake of completeness, this module also propose the two following classes: 28 | * - class CholmodSimplicialLLT 29 | * - class CholmodSimplicialLDLT 30 | * Note that these classes does not bring any particular advantage compared to the built-in 31 | * SimplicialLLT and SimplicialLDLT factorization classes. 32 | * 33 | * \code 34 | * #include 35 | * \endcode 36 | * 37 | * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. 38 | * The dependencies depend on how cholmod has been compiled. 39 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 40 | * 41 | */ 42 | 43 | #include "src/CholmodSupport/CholmodSupport.h" 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 48 | 49 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/Default/ConjHelper.h: -------------------------------------------------------------------------------- 1 | 2 | // This file is part of Eigen, a lightweight C++ template library 3 | // for linear algebra. 4 | // 5 | // Copyright (C) 2017 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_ARCH_CONJ_HELPER_H 12 | #define EIGEN_ARCH_CONJ_HELPER_H 13 | 14 | #define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL) \ 15 | template<> struct conj_helper { \ 16 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const \ 17 | { return padd(c, pmul(x,y)); } \ 18 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const \ 19 | { return PACKET_CPLX(Eigen::internal::pmul(x, y.v)); } \ 20 | }; \ 21 | \ 22 | template<> struct conj_helper { \ 23 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const \ 24 | { return padd(c, pmul(x,y)); } \ 25 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const \ 26 | { return PACKET_CPLX(Eigen::internal::pmul(x.v, y)); } \ 27 | }; 28 | 29 | #endif // EIGEN_ARCH_CONJ_HELPER_H 30 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/SSE/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_SSE_H 11 | #define EIGEN_TYPE_CASTING_SSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | #ifndef EIGEN_VECTORIZE_AVX 18 | template <> 19 | struct type_casting_traits { 20 | enum { 21 | VectorizedCast = 1, 22 | SrcCoeffRatio = 1, 23 | TgtCoeffRatio = 1 24 | }; 25 | }; 26 | 27 | template <> 28 | struct type_casting_traits { 29 | enum { 30 | VectorizedCast = 1, 31 | SrcCoeffRatio = 1, 32 | TgtCoeffRatio = 1 33 | }; 34 | }; 35 | 36 | template <> 37 | struct type_casting_traits { 38 | enum { 39 | VectorizedCast = 1, 40 | SrcCoeffRatio = 2, 41 | TgtCoeffRatio = 1 42 | }; 43 | }; 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 1, 50 | TgtCoeffRatio = 2 51 | }; 52 | }; 53 | #endif 54 | 55 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 56 | return _mm_cvttps_epi32(a); 57 | } 58 | 59 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 60 | return _mm_cvtepi32_ps(a); 61 | } 62 | 63 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 64 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 65 | } 66 | 67 | template<> EIGEN_STRONG_INLINE Packet2d pcast(const Packet4f& a) { 68 | // Simply discard the second half of the input 69 | return _mm_cvtps_pd(a); 70 | } 71 | 72 | 73 | } // end namespace internal 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_TYPE_CASTING_SSE_H 78 | -------------------------------------------------------------------------------- /include/Eigen_embedded/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 9 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** 17 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 18 | * 19 | * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. 20 | * Those solvers are accessible via the following classes: 21 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 22 | * - LeastSquaresConjugateGradient for rectangular least-square problems, 23 | * - BiCGSTAB for general square matrices. 24 | * 25 | * These iterative solvers are associated with some preconditioners: 26 | * - IdentityPreconditioner - not really useful 27 | * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. 28 | * - IncompleteLUT - incomplete LU factorization with dual thresholding 29 | * 30 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 31 | * 32 | \code 33 | #include 34 | \endcode 35 | */ 36 | 37 | #include "src/IterativeLinearSolvers/SolveWithGuess.h" 38 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 39 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 40 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 41 | #include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" 42 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 43 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 44 | #include "src/IterativeLinearSolvers/IncompleteCholesky.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 49 | -------------------------------------------------------------------------------- /launch/hardware_mpc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /include/Eigen_embedded/Geometry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_GEOMETRY_MODULE_H 9 | #define EIGEN_GEOMETRY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SVD" 16 | #include "LU" 17 | #include 18 | 19 | /** \defgroup Geometry_Module Geometry module 20 | * 21 | * This module provides support for: 22 | * - fixed-size homogeneous transformations 23 | * - translation, scaling, 2D and 3D rotations 24 | * - \link Quaternion quaternions \endlink 25 | * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) 26 | * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) 27 | * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink 28 | * - \link AlignedBox axis aligned bounding boxes \endlink 29 | * - \link umeyama least-square transformation fitting \endlink 30 | * 31 | * \code 32 | * #include 33 | * \endcode 34 | */ 35 | 36 | #include "src/Geometry/OrthoMethods.h" 37 | #include "src/Geometry/EulerAngles.h" 38 | 39 | #include "src/Geometry/Homogeneous.h" 40 | #include "src/Geometry/RotationBase.h" 41 | #include "src/Geometry/Rotation2D.h" 42 | #include "src/Geometry/Quaternion.h" 43 | #include "src/Geometry/AngleAxis.h" 44 | #include "src/Geometry/Transform.h" 45 | #include "src/Geometry/Translation.h" 46 | #include "src/Geometry/Scaling.h" 47 | #include "src/Geometry/Hyperplane.h" 48 | #include "src/Geometry/ParametrizedLine.h" 49 | #include "src/Geometry/AlignedBox.h" 50 | #include "src/Geometry/Umeyama.h" 51 | 52 | // Use the SSE optimized version whenever possible. At the moment the 53 | // SSE version doesn't compile when AVX is enabled 54 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 55 | #include "src/Geometry/arch/Geometry_SSE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_GEOMETRY_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | 63 | -------------------------------------------------------------------------------- /URDF/cyberpod.urdf: -------------------------------------------------------------------------------- 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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseLU/SparseLU_Utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | 11 | #ifndef EIGEN_SPARSELU_UTILS_H 12 | #define EIGEN_SPARSELU_UTILS_H 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | /** 18 | * \brief Count Nonzero elements in the factors 19 | */ 20 | template 21 | void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) 22 | { 23 | nnzL = 0; 24 | nnzU = (glu.xusub)(n); 25 | Index nsuper = (glu.supno)(n); 26 | Index jlen; 27 | Index i, j, fsupc; 28 | if (n <= 0 ) return; 29 | // For each supernode 30 | for (i = 0; i <= nsuper; i++) 31 | { 32 | fsupc = glu.xsup(i); 33 | jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 34 | 35 | for (j = fsupc; j < glu.xsup(i+1); j++) 36 | { 37 | nnzL += jlen; 38 | nnzU += j - fsupc + 1; 39 | jlen--; 40 | } 41 | } 42 | } 43 | 44 | /** 45 | * \brief Fix up the data storage lsub for L-subscripts. 46 | * 47 | * It removes the subscripts sets for structural pruning, 48 | * and applies permutation to the remaining subscripts 49 | * 50 | */ 51 | template 52 | void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) 53 | { 54 | Index fsupc, i, j, k, jstart; 55 | 56 | StorageIndex nextl = 0; 57 | Index nsuper = (glu.supno)(n); 58 | 59 | // For each supernode 60 | for (i = 0; i <= nsuper; i++) 61 | { 62 | fsupc = glu.xsup(i); 63 | jstart = glu.xlsub(fsupc); 64 | glu.xlsub(fsupc) = nextl; 65 | for (j = jstart; j < glu.xlsub(fsupc + 1); j++) 66 | { 67 | glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A 68 | nextl++; 69 | } 70 | for (k = fsupc+1; k < glu.xsup(i+1); k++) 71 | glu.xlsub(k) = nextl; // other columns in supernode i 72 | } 73 | 74 | glu.xlsub(n) = nextl; 75 | } 76 | 77 | } // end namespace internal 78 | 79 | } // end namespace Eigen 80 | #endif // EIGEN_SPARSELU_UTILS_H 81 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseCore/MappedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MAPPED_SPARSEMATRIX_H 11 | #define EIGEN_MAPPED_SPARSEMATRIX_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated Use Map > 16 | * \class MappedSparseMatrix 17 | * 18 | * \brief Sparse matrix 19 | * 20 | * \param _Scalar the scalar type, i.e. the type of the coefficients 21 | * 22 | * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. 23 | * 24 | */ 25 | namespace internal { 26 | template 27 | struct traits > : traits > 28 | {}; 29 | } // end namespace internal 30 | 31 | template 32 | class MappedSparseMatrix 33 | : public Map > 34 | { 35 | typedef Map > Base; 36 | 37 | public: 38 | 39 | typedef typename Base::StorageIndex StorageIndex; 40 | typedef typename Base::Scalar Scalar; 41 | 42 | inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) 43 | : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) 44 | {} 45 | 46 | /** Empty destructor */ 47 | inline ~MappedSparseMatrix() {} 48 | }; 49 | 50 | namespace internal { 51 | 52 | template 53 | struct evaluator > 54 | : evaluator > > 55 | { 56 | typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; 57 | typedef evaluator > Base; 58 | 59 | evaluator() : Base() {} 60 | explicit evaluator(const XprType &mat) : Base(mat) {} 61 | }; 62 | 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_MAPPED_SPARSEMATRIX_H 68 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SuperLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 9 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #ifdef EMPTY 16 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 17 | #endif 18 | 19 | typedef int int_t; 20 | #include 21 | #include 22 | #include 23 | 24 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 25 | // so we remove it in favor of a SUPERLU_EMPTY token. 26 | // If EMPTY was already defined then we don't undef it. 27 | 28 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 29 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 30 | #elif defined(EMPTY) 31 | # undef EMPTY 32 | #endif 33 | 34 | #define SUPERLU_EMPTY (-1) 35 | 36 | namespace Eigen { struct SluMatrix; } 37 | 38 | /** \ingroup Support_modules 39 | * \defgroup SuperLUSupport_Module SuperLUSupport module 40 | * 41 | * This module provides an interface to the SuperLU library. 42 | * It provides the following factorization class: 43 | * - class SuperLU: a supernodal sequential LU factorization. 44 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 45 | * 46 | * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. 47 | * 48 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 49 | * 50 | * \code 51 | * #include 52 | * \endcode 53 | * 54 | * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. 55 | * The dependencies depend on how superlu has been compiled. 56 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 57 | * 58 | */ 59 | 60 | #include "src/SuperLUSupport/SuperLUSupport.h" 61 | 62 | #include "src/Core/util/ReenableStupidWarnings.h" 63 | 64 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 65 | -------------------------------------------------------------------------------- /include/Eigen_embedded/SparseCore: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSECORE_MODULE_H 9 | #define EIGEN_SPARSECORE_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | /** 22 | * \defgroup SparseCore_Module SparseCore module 23 | * 24 | * This module provides a sparse matrix representation, and basic associated matrix manipulations 25 | * and operations. 26 | * 27 | * See the \ref TutorialSparse "Sparse tutorial" 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | * 33 | * This module depends on: Core. 34 | */ 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/SparseAssign.h" 39 | #include "src/SparseCore/CompressedStorage.h" 40 | #include "src/SparseCore/AmbiVector.h" 41 | #include "src/SparseCore/SparseCompressedBase.h" 42 | #include "src/SparseCore/SparseMatrix.h" 43 | #include "src/SparseCore/SparseMap.h" 44 | #include "src/SparseCore/MappedSparseMatrix.h" 45 | #include "src/SparseCore/SparseVector.h" 46 | #include "src/SparseCore/SparseRef.h" 47 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 48 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 49 | #include "src/SparseCore/SparseTranspose.h" 50 | #include "src/SparseCore/SparseBlock.h" 51 | #include "src/SparseCore/SparseDot.h" 52 | #include "src/SparseCore/SparseRedux.h" 53 | #include "src/SparseCore/SparseView.h" 54 | #include "src/SparseCore/SparseDiagonalProduct.h" 55 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 56 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 57 | #include "src/SparseCore/SparseProduct.h" 58 | #include "src/SparseCore/SparseDenseProduct.h" 59 | #include "src/SparseCore/SparseSelfAdjointView.h" 60 | #include "src/SparseCore/SparseTriangularView.h" 61 | #include "src/SparseCore/TriangularSolver.h" 62 | #include "src/SparseCore/SparsePermutation.h" 63 | #include "src/SparseCore/SparseFuzzy.h" 64 | #include "src/SparseCore/SparseSolverBase.h" 65 | 66 | #include "src/Core/util/ReenableStupidWarnings.h" 67 | 68 | #endif // EIGEN_SPARSECORE_MODULE_H 69 | 70 | -------------------------------------------------------------------------------- /src/ekf.cpp: -------------------------------------------------------------------------------- 1 | #include "segway_sim/ekf.h" 2 | 3 | typedef Eigen::Matrix MatrixNN; 4 | typedef Eigen::Matrix MatrixZN; 5 | typedef Eigen::Matrix MatrixNZ; 6 | typedef Eigen::Matrix MatrixZZ; 7 | typedef Eigen::Matrix VectorN; 8 | typedef Eigen::Matrix VectorZ; 9 | typedef Eigen::Map MapMatrixNN; 10 | typedef Eigen::Map MapMatrixZN; 11 | 12 | EKF::EKF(const std::vector &Q, 13 | const std::vector &R): 14 | Q_(Q), 15 | R_(R) 16 | {} 17 | 18 | void EKF::init(const std::vector &x0, const std::vector &P0vec) 19 | { 20 | x_t_ = x0; 21 | P_t_.resize(STATE_LENGTH*STATE_LENGTH); 22 | MapMatrixNN P(P_t_.data()); 23 | P.setZero(); 24 | for (int i = 0; i < (int)P0vec.size(); i ++) { 25 | P(i,i) = P0vec[i]; 26 | } 27 | initialized = 1; 28 | } 29 | 30 | void EKF::update(const std::vector &U, 31 | const std::vector &M, 32 | const double dt) 33 | { 34 | //predict x 35 | double xDot[STATE_LENGTH]; 36 | dynamics(0.,x_t_.data(),U.data(),xDot); 37 | double xPred[STATE_LENGTH] = {0.}; 38 | for (int i = 0; i < (int)STATE_LENGTH; i++) { 39 | xPred[i] = x_t_[i]+xDot[i]*dt; 40 | } 41 | 42 | //get F 43 | double DfRaw[STATE_LENGTH*STATE_LENGTH]; 44 | dynamicsGradient(0., x_t_.data(), U.data(), DfRaw); 45 | MapMatrixNN Df(DfRaw); 46 | 47 | //get h and H 48 | double h[MEASUREMENT_LENGTH]; 49 | measurementH(xPred,h); 50 | double DhRaw[MEASUREMENT_LENGTH*STATE_LENGTH]; 51 | measurementHGradient(xPred,DhRaw); 52 | MapMatrixZN Dh(DhRaw); 53 | 54 | //setup Q and R 55 | MatrixNN Q; 56 | Q.setZero(); 57 | 58 | MatrixZZ R; 59 | R.setZero(); 60 | 61 | for (int i = 0; i < STATE_LENGTH; i++) { 62 | Q(i,i) = Q_[i]; 63 | } 64 | 65 | for (int i = 0; i < MEASUREMENT_LENGTH; i++) { 66 | R(i,i) = R_[i]; 67 | } 68 | 69 | //predict P 70 | const Eigen::Map P_old(P_t_.data()); 71 | MatrixNN P = P_old + dt*(Df*P_old+P_old*Df.transpose()+Q); 72 | 73 | //map xPred and z and h 74 | const Eigen::Map z(M.data()); 75 | 76 | const Eigen::Map h_mat(h); 77 | const Eigen::Map xPred_mat(xPred); 78 | 79 | //compute K 80 | MatrixNZ K = P*Dh.transpose()*(Dh*P*Dh.transpose()+R).inverse(); 81 | // 82 | // //compute x 83 | Eigen::Map X_new(x_t_.data()); 84 | X_new = xPred_mat + K*(z-h_mat); 85 | // 86 | // //compute P 87 | MapMatrixNN P_new(P_t_.data()); 88 | P_new = ((MatrixNN::Identity()-K*Dh)*P).eval(); 89 | initialized = 1; 90 | } 91 | -------------------------------------------------------------------------------- /include/Eigen_embedded.h: -------------------------------------------------------------------------------- 1 | /* 2 | Eigen.h 3 | Brian R Taylor 4 | brian.taylor@bolderflight.com 5 | 2017-02-08 6 | 7 | Copyright (c) 2017 Bolder Flight Systems 8 | 9 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software 10 | and associated documentation files (the "Software"), to deal in the Software without restriction, 11 | including without limitation the rights to use, copy, modify, merge, publish, distribute, 12 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 13 | furnished to do so, subject to the following conditions: 14 | 15 | The above copyright notice and this permission notice shall be included in all copies or 16 | substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 19 | BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 20 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 21 | DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | */ 24 | 25 | // Credits: @rpavlik for writing the original header for the Eigen313 library, which this 26 | // was derived from: 27 | // http://forum.arduino.cc/index.php?PHPSESSID=a86gv50nb3e3ireijfmli63260&topic=144446.msg1089371#msg1089371 28 | 29 | // Disable debug asserts. 30 | #define EIGEN_NO_DEBUG 1 31 | 32 | // Hint to number of registers 33 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 16 34 | 35 | #ifdef A0 36 | # define NEED_A0_RESTORED A0 37 | # undef A0 38 | #endif 39 | #ifdef A1 40 | # define NEED_A1_RESTORED A1 41 | # undef A1 42 | #endif 43 | #ifdef B0 44 | # define NEED_B0_RESTORED B0 45 | # undef B0 46 | #endif 47 | #ifdef B1 48 | # define NEED_B1_RESTORED B1 49 | # undef B1 50 | #endif 51 | #ifdef round 52 | # define NEED_round_RESTORED round 53 | # undef round 54 | #endif 55 | 56 | namespace std { 57 | struct nothrow_t; 58 | } 59 | 60 | // Include main EIGEN Core header 61 | #include "Eigen_embedded/Core" 62 | 63 | #ifdef NEED_A0_RESTORED 64 | # define A0 NEED_A0_RESTORED 65 | # undef NEED_A0_RESTORED 66 | #endif 67 | #ifdef NEED_A1_RESTORED 68 | # define A1 NEED_A1_RESTORED 69 | # undef NEED_A1_RESTORED 70 | #endif 71 | #ifdef NEED_B0_RESTORED 72 | # define B0 NEED_B0_RESTORED 73 | # undef NEED_B0_RESTORED 74 | #endif 75 | #ifdef NEED_B1_RESTORED 76 | # define B1 NEED_B1_RESTORED 77 | # undef NEED_B1_RESTORED 78 | #endif 79 | #ifdef NEED_round_RESTORED 80 | # define round NEED_round_RESTORED 81 | # undef NEED_round_RESTORED 82 | #endif 83 | -------------------------------------------------------------------------------- /src/ekf_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "segway_sim/state.h" 3 | #include "segway_sim/sensor.h" 4 | #include "segway_sim/input.h" 5 | #include "segway_sim/ekf.h" 6 | 7 | using namespace Eigen; 8 | 9 | ros::NodeHandle *nh_; 10 | ros::NodeHandle *nhParams_; 11 | ros::ServiceServer srv_ui_; 12 | ros::Subscriber sub_sensor_; 13 | ros::Subscriber sub_input_; 14 | ros::Publisher pub_state_; 15 | EKF *ekf_ = nullptr; 16 | segway_sim::input inputCurrent_; 17 | segway_sim::sensor sensorCurrent_; 18 | 19 | double Rw_ = 0.195; 20 | double L_ = 0.5; 21 | double dt_ = 0; 22 | 23 | void inputCallback(const segway_sim::input::ConstPtr msg) 24 | { 25 | inputCurrent_ = *msg; 26 | } 27 | 28 | void sensorCallback(const segway_sim::sensor::ConstPtr msg) 29 | { 30 | dt_ = (msg->time-sensorCurrent_.time)*1E-6; 31 | sensorCurrent_ = *msg; 32 | double v = Rw_*(msg->data[2]+msg->data[3])/2; 33 | double thetaDot = msg->data[4]; 34 | double psi = msg->data[5]; 35 | double psiDot = msg->data[6]; 36 | if (ekf_->initialized == 0) { 37 | std::vector state_ekf_ = {0,0,0,v,thetaDot,psi,psiDot}; 38 | std::vector P_ekf_ = {0.,0.,0.,0.01,0.01,0.01,0.01}; 39 | ekf_->init(state_ekf_,P_ekf_); 40 | } 41 | else if (ekf_->initialized == 1){ 42 | ekf_->update(std::vector {inputCurrent_.inputVec[0],inputCurrent_.inputVec[1]}, 43 | std::vector {v,thetaDot,psi,psiDot}, 44 | dt_); 45 | } 46 | segway_sim::state state_msg; 47 | state_msg.time = sensorCurrent_.time; 48 | state_msg.x = ekf_->x_t_[0]; 49 | state_msg.y = ekf_->x_t_[1]; 50 | state_msg.theta = ekf_->x_t_[2]; 51 | state_msg.v = ekf_->x_t_[3]; 52 | state_msg.thetaDot = ekf_->x_t_[4]; 53 | state_msg.psi = ekf_->x_t_[5]; 54 | state_msg.psiDot = ekf_->x_t_[6]; 55 | 56 | for (int i = 0 ; i < 7; i ++) { 57 | state_msg.stateVec[i] = ekf_->x_t_[i]; 58 | } 59 | 60 | pub_state_.publish(state_msg); 61 | } 62 | 63 | int main (int argc, char *argv[]) 64 | { 65 | ekf_ = new EKF(std::vector {.01,.01,.01,.01,.01,.01,.01}, 66 | std::vector {.001,.001,.001,.001}); 67 | // Init ros 68 | ros::init(argc,argv,"ekf_node"); 69 | 70 | // Instanciate NodeHandles 71 | nhParams_ = new ros::NodeHandle("~"); 72 | nh_ = new ros::NodeHandle(); 73 | 74 | // Init pubs, subs and srvs 75 | sub_input_ = nh_->subscribe("input", 1,inputCallback); 76 | sub_sensor_ = nh_->subscribe("sensor", 1,sensorCallback); 77 | pub_state_ = nh_->advertise("state", 1); 78 | 79 | // Retreive params 80 | // nhParams_->param("enc_v_variance",enc_v_variance_,0.001); 81 | 82 | // Display node info 83 | ROS_INFO("EKF node successfuly started!"); 84 | 85 | ros::spin(); 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /include/Eigen_embedded/OrderingMethods: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ORDERINGMETHODS_MODULE_H 9 | #define EIGEN_ORDERINGMETHODS_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** 16 | * \defgroup OrderingMethods_Module OrderingMethods module 17 | * 18 | * This module is currently for internal use only 19 | * 20 | * It defines various built-in and external ordering methods for sparse matrices. 21 | * They are typically used to reduce the number of elements during 22 | * the sparse matrix decomposition (LLT, LU, QR). 23 | * Precisely, in a preprocessing step, a permutation matrix P is computed using 24 | * those ordering methods and applied to the columns of the matrix. 25 | * Using for instance the sparse Cholesky decomposition, it is expected that 26 | * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). 27 | * 28 | * 29 | * Usage : 30 | * \code 31 | * #include 32 | * \endcode 33 | * 34 | * A simple usage is as a template parameter in the sparse decomposition classes : 35 | * 36 | * \code 37 | * SparseLU > solver; 38 | * \endcode 39 | * 40 | * \code 41 | * SparseQR > solver; 42 | * \endcode 43 | * 44 | * It is possible as well to call directly a particular ordering method for your own purpose, 45 | * \code 46 | * AMDOrdering ordering; 47 | * PermutationMatrix perm; 48 | * SparseMatrix A; 49 | * //Fill the matrix ... 50 | * 51 | * ordering(A, perm); // Call AMD 52 | * \endcode 53 | * 54 | * \note Some of these methods (like AMD or METIS), need the sparsity pattern 55 | * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 56 | * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. 57 | * If your matrix is already symmetric (at leat in structure), you can avoid that 58 | * by calling the method with a SelfAdjointView type. 59 | * 60 | * \code 61 | * // Call the ordering on the pattern of the lower triangular matrix A 62 | * ordering(A.selfadjointView(), perm); 63 | * \endcode 64 | */ 65 | 66 | #ifndef EIGEN_MPL2_ONLY 67 | #include "src/OrderingMethods/Amd.h" 68 | #endif 69 | 70 | #include "src/OrderingMethods/Ordering.h" 71 | #include "src/Core/util/ReenableStupidWarnings.h" 72 | 73 | #endif // EIGEN_ORDERINGMETHODS_MODULE_H 74 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/CUDA/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MATH_FUNCTIONS_CUDA_H 11 | #define EIGEN_MATH_FUNCTIONS_CUDA_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Make sure this is only available when targeting a GPU: we don't want to 18 | // introduce conflicts between these packet_traits definitions and the ones 19 | // we'll use on the host side (SSE, AVX, ...) 20 | #if defined(__CUDACC__) && defined(EIGEN_USE_GPU) 21 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 22 | float4 plog(const float4& a) 23 | { 24 | return make_float4(logf(a.x), logf(a.y), logf(a.z), logf(a.w)); 25 | } 26 | 27 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 28 | double2 plog(const double2& a) 29 | { 30 | using ::log; 31 | return make_double2(log(a.x), log(a.y)); 32 | } 33 | 34 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 35 | float4 plog1p(const float4& a) 36 | { 37 | return make_float4(log1pf(a.x), log1pf(a.y), log1pf(a.z), log1pf(a.w)); 38 | } 39 | 40 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 41 | double2 plog1p(const double2& a) 42 | { 43 | return make_double2(log1p(a.x), log1p(a.y)); 44 | } 45 | 46 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 47 | float4 pexp(const float4& a) 48 | { 49 | return make_float4(expf(a.x), expf(a.y), expf(a.z), expf(a.w)); 50 | } 51 | 52 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 53 | double2 pexp(const double2& a) 54 | { 55 | using ::exp; 56 | return make_double2(exp(a.x), exp(a.y)); 57 | } 58 | 59 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 60 | float4 psqrt(const float4& a) 61 | { 62 | return make_float4(sqrtf(a.x), sqrtf(a.y), sqrtf(a.z), sqrtf(a.w)); 63 | } 64 | 65 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 66 | double2 psqrt(const double2& a) 67 | { 68 | using ::sqrt; 69 | return make_double2(sqrt(a.x), sqrt(a.y)); 70 | } 71 | 72 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 73 | float4 prsqrt(const float4& a) 74 | { 75 | return make_float4(rsqrtf(a.x), rsqrtf(a.y), rsqrtf(a.z), rsqrtf(a.w)); 76 | } 77 | 78 | template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 79 | double2 prsqrt(const double2& a) 80 | { 81 | return make_double2(rsqrt(a.x), rsqrt(a.y)); 82 | } 83 | 84 | 85 | #endif 86 | 87 | } // end namespace internal 88 | 89 | } // end namespace Eigen 90 | 91 | #endif // EIGEN_MATH_FUNCTIONS_CUDA_H 92 | -------------------------------------------------------------------------------- /src/sensor_node.cpp: -------------------------------------------------------------------------------- 1 | #include "segway_sim/sensor_node.hpp" 2 | 3 | 4 | using namespace Eigen; 5 | 6 | ros::NodeHandle *nh_; 7 | ros::NodeHandle *nhParams_; 8 | ros::ServiceServer srv_ui_; 9 | ros::Subscriber sub_state_; 10 | ros::Publisher pub_sensor_; 11 | 12 | double enc_v_variance_,psi_variance_,psi_dot_variance_,theta_dot_variance_; 13 | segway_sim::state state_current_; 14 | double dt_,v_last_,theta_last_; 15 | double encL_pos_,encR_pos_; 16 | double Rw_ = 0.195; 17 | double L_ = 0.5; 18 | 19 | void stateCallback(const segway_sim::state::ConstPtr msg) 20 | { 21 | //compute sensor values (no noise) 22 | state_current_ = *msg; 23 | double encoderL_vel = (state_current_.v - state_current_.thetaDot*L_)/Rw_; 24 | double encoderR_vel = (state_current_.v + state_current_.thetaDot*L_)/Rw_; 25 | 26 | double thetaDot = state_current_.thetaDot; 27 | 28 | double psi = state_current_.psi; 29 | double psiDot = state_current_.psiDot; 30 | 31 | double steering = 0.; 32 | double battV = 60.; 33 | 34 | // add in random noise 35 | encoderL_vel += ( ((double)rand()/(double)RAND_MAX) *2-1)*enc_v_variance_; 36 | encoderR_vel += ( ((double)rand()/(double)RAND_MAX) *2-1)*enc_v_variance_; 37 | psi += ( ((double)rand()/(double)RAND_MAX) *2-1)*psi_variance_; 38 | psiDot += ( ((double)rand()/(double)RAND_MAX) *2-1)*psi_dot_variance_; 39 | thetaDot += ( ((double)rand()/(double)RAND_MAX) *2-1)*theta_dot_variance_; 40 | 41 | //update encoder positions; 42 | encL_pos_ += encoderL_vel*dt_; 43 | encR_pos_ += encoderR_vel*dt_; 44 | 45 | segway_sim::sensor sensor_msg; 46 | 47 | sensor_msg.time = (uint32_t) (state_current_.time*1E6); 48 | sensor_msg.data.push_back(encL_pos_); 49 | sensor_msg.data.push_back(encR_pos_); 50 | sensor_msg.data.push_back(encoderL_vel); 51 | sensor_msg.data.push_back(encoderR_vel); 52 | sensor_msg.data.push_back(thetaDot); 53 | sensor_msg.data.push_back(psi); 54 | sensor_msg.data.push_back(psiDot); 55 | sensor_msg.data.push_back(steering); 56 | sensor_msg.data.push_back(battV); 57 | 58 | pub_sensor_.publish(sensor_msg); 59 | } 60 | 61 | int main (int argc, char *argv[]) 62 | { 63 | // Init ros 64 | ros::init(argc,argv,"sensor"); 65 | 66 | // Instanciate NodeHandles 67 | nhParams_ = new ros::NodeHandle("~"); 68 | nh_ = new ros::NodeHandle(); 69 | 70 | // Init pubs, subs and srvs 71 | sub_state_ = nh_->subscribe("state_true", 1,stateCallback); 72 | pub_sensor_ = nh_->advertise("sensor", 1); 73 | 74 | // Retreive params 75 | nhParams_->param("enc_v_variance",enc_v_variance_,0.001); 76 | nhParams_->param("psi_variance",psi_variance_,0.001); 77 | nhParams_->param("psi_dot_variance",psi_dot_variance_,0.001); 78 | nhParams_->param("theta_dot_variance",theta_dot_variance_,0.001); 79 | 80 | // Display node info 81 | ROS_INFO("Sensor node successfuly started!"); 82 | 83 | ros::spin(); 84 | 85 | return 0; 86 | } 87 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/Swap.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2006-2008 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SWAP_H 11 | #define EIGEN_SWAP_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // Overload default assignPacket behavior for swapping them 18 | template 19 | class generic_dense_assignment_kernel, Specialized> 20 | : public generic_dense_assignment_kernel, BuiltIn> 21 | { 22 | protected: 23 | typedef generic_dense_assignment_kernel, BuiltIn> Base; 24 | using Base::m_dst; 25 | using Base::m_src; 26 | using Base::m_functor; 27 | 28 | public: 29 | typedef typename Base::Scalar Scalar; 30 | typedef typename Base::DstXprType DstXprType; 31 | typedef swap_assign_op Functor; 32 | 33 | EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr) 34 | : Base(dst, src, func, dstExpr) 35 | {} 36 | 37 | template 38 | void assignPacket(Index row, Index col) 39 | { 40 | PacketType tmp = m_src.template packet(row,col); 41 | const_cast(m_src).template writePacket(row,col, m_dst.template packet(row,col)); 42 | m_dst.template writePacket(row,col,tmp); 43 | } 44 | 45 | template 46 | void assignPacket(Index index) 47 | { 48 | PacketType tmp = m_src.template packet(index); 49 | const_cast(m_src).template writePacket(index, m_dst.template packet(index)); 50 | m_dst.template writePacket(index,tmp); 51 | } 52 | 53 | // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I mean no CRTP (Gael) 54 | template 55 | void assignPacketByOuterInner(Index outer, Index inner) 56 | { 57 | Index row = Base::rowIndexByOuterInner(outer, inner); 58 | Index col = Base::colIndexByOuterInner(outer, inner); 59 | assignPacket(row, col); 60 | } 61 | }; 62 | 63 | } // namespace internal 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_SWAP_H 68 | -------------------------------------------------------------------------------- /src/controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include "segway_sim/controller_node.hpp" 2 | 3 | using namespace Eigen; 4 | 5 | ros::NodeHandle *nh_; 6 | ros::NodeHandle *nhParams_; 7 | ros::Subscriber sub_state_; 8 | ros::Subscriber sub_cmd_; 9 | ros::Publisher pub_input_; 10 | 11 | segway_sim::input input_; 12 | segway_sim::state stateCurrent_; 13 | segway_sim::cmd cmdCurrent_; 14 | 15 | // Global variables 16 | uint32_t iter_; 17 | VectorXd gainsVec_(STATE_LENGTH); 18 | VectorXd stateCurrentVec_(STATE_LENGTH); 19 | double offset_angle_; 20 | 21 | // Controller gains 22 | std::vector gains_(STATE_LENGTH,0.0); 23 | 24 | 25 | void computeControlAction(void) 26 | { 27 | // Prepare Header 28 | iter_++; 29 | input_.header.seq = iter_; 30 | input_.header.stamp = ros::Time::now(); 31 | input_.header.frame_id = std::string("stateSeq=") + std::to_string(stateCurrent_.header.seq) + std::string(", cmdSeq=") + std::to_string(cmdCurrent_.header.seq); 32 | input_.status = static_cast(STATUS::RUNNING); 33 | 34 | VectorXd stateDes(STATE_LENGTH); 35 | stateDes(5)=offset_angle_; 36 | stateDes(3)=cmdCurrent_.cmd[0]; 37 | 38 | const double u = gainsVec_.dot(stateCurrentVec_ - stateDes); 39 | 40 | input_.inputVec[0] = u; 41 | input_.inputVec[1] = u; 42 | } 43 | 44 | void controlCallback(const segway_sim::state::ConstPtr msg) 45 | { 46 | stateCurrent_ = *msg; 47 | for(uint32_t i=0; isubscribe("state", 1, controlCallback); 75 | sub_cmd_ = nh_->subscribe("cmd", 1, cmdCallback); 76 | pub_input_ = nh_->advertise("inputDes", 1); 77 | 78 | // Retreive params 79 | nhParams_->param("offset_angle",offset_angle_,0.); 80 | nhParams_->param>("gains",gains_,gains_); 81 | if(gains_.size()!=STATE_LENGTH) 82 | { 83 | gains_ = std::vector(STATE_LENGTH,0.0); 84 | ROS_WARN("gains must be of length %u",STATE_LENGTH); 85 | } 86 | for(uint32_t i=0; i 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_KERNEL_H 11 | #define EIGEN_MISC_KERNEL_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class kernel_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix< 25 | typename MatrixType::Scalar, 26 | MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" 27 | // is the number of cols of the original matrix 28 | // so that the product "matrix * kernel = zero" makes sense 29 | Dynamic, // we don't know at compile-time the dimension of the kernel 30 | MatrixType::Options, 31 | MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter 32 | MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, 33 | // whose dimension is the number of columns of the original matrix 34 | > ReturnType; 35 | }; 36 | 37 | template struct kernel_retval_base 38 | : public ReturnByValue > 39 | { 40 | typedef _DecompositionType DecompositionType; 41 | typedef ReturnByValue Base; 42 | 43 | explicit kernel_retval_base(const DecompositionType& dec) 44 | : m_dec(dec), 45 | m_rank(dec.rank()), 46 | m_cols(m_rank==dec.cols() ? 1 : dec.cols() - m_rank) 47 | {} 48 | 49 | inline Index rows() const { return m_dec.cols(); } 50 | inline Index cols() const { return m_cols; } 51 | inline Index rank() const { return m_rank; } 52 | inline const DecompositionType& dec() const { return m_dec; } 53 | 54 | template inline void evalTo(Dest& dst) const 55 | { 56 | static_cast*>(this)->evalTo(dst); 57 | } 58 | 59 | protected: 60 | const DecompositionType& m_dec; 61 | Index m_rank, m_cols; 62 | }; 63 | 64 | } // end namespace internal 65 | 66 | #define EIGEN_MAKE_KERNEL_HELPERS(DecompositionType) \ 67 | typedef typename DecompositionType::MatrixType MatrixType; \ 68 | typedef typename MatrixType::Scalar Scalar; \ 69 | typedef typename MatrixType::RealScalar RealScalar; \ 70 | typedef Eigen::internal::kernel_retval_base Base; \ 71 | using Base::dec; \ 72 | using Base::rank; \ 73 | using Base::rows; \ 74 | using Base::cols; \ 75 | kernel_retval(const DecompositionType& dec) : Base(dec) {} 76 | 77 | } // end namespace Eigen 78 | 79 | #endif // EIGEN_MISC_KERNEL_H 80 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/Assign.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2007 Michael Olbrich 5 | // Copyright (C) 2006-2010 Benoit Jacob 6 | // Copyright (C) 2008 Gael Guennebaud 7 | // 8 | // This Source Code Form is subject to the terms of the Mozilla 9 | // Public License v. 2.0. If a copy of the MPL was not distributed 10 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 11 | 12 | #ifndef EIGEN_ASSIGN_H 13 | #define EIGEN_ASSIGN_H 14 | 15 | namespace Eigen { 16 | 17 | template 18 | template 19 | EIGEN_STRONG_INLINE Derived& DenseBase 20 | ::lazyAssign(const DenseBase& other) 21 | { 22 | enum{ 23 | SameType = internal::is_same::value 24 | }; 25 | 26 | EIGEN_STATIC_ASSERT_LVALUE(Derived) 27 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived) 28 | EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 29 | 30 | eigen_assert(rows() == other.rows() && cols() == other.cols()); 31 | internal::call_assignment_no_alias(derived(),other.derived()); 32 | 33 | return derived(); 34 | } 35 | 36 | template 37 | template 38 | EIGEN_DEVICE_FUNC 39 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 40 | { 41 | internal::call_assignment(derived(), other.derived()); 42 | return derived(); 43 | } 44 | 45 | template 46 | EIGEN_DEVICE_FUNC 47 | EIGEN_STRONG_INLINE Derived& DenseBase::operator=(const DenseBase& other) 48 | { 49 | internal::call_assignment(derived(), other.derived()); 50 | return derived(); 51 | } 52 | 53 | template 54 | EIGEN_DEVICE_FUNC 55 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const MatrixBase& other) 56 | { 57 | internal::call_assignment(derived(), other.derived()); 58 | return derived(); 59 | } 60 | 61 | template 62 | template 63 | EIGEN_DEVICE_FUNC 64 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const DenseBase& other) 65 | { 66 | internal::call_assignment(derived(), other.derived()); 67 | return derived(); 68 | } 69 | 70 | template 71 | template 72 | EIGEN_DEVICE_FUNC 73 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const EigenBase& other) 74 | { 75 | internal::call_assignment(derived(), other.derived()); 76 | return derived(); 77 | } 78 | 79 | template 80 | template 81 | EIGEN_DEVICE_FUNC 82 | EIGEN_STRONG_INLINE Derived& MatrixBase::operator=(const ReturnByValue& other) 83 | { 84 | other.derived().evalTo(derived()); 85 | return derived(); 86 | } 87 | 88 | } // end namespace Eigen 89 | 90 | #endif // EIGEN_ASSIGN_H 91 | -------------------------------------------------------------------------------- /src/pyFun/mainMulti.py: -------------------------------------------------------------------------------- 1 | import pdb 2 | import scipy.io as sio 3 | import numpy as np 4 | import pickle 5 | import sys 6 | from utils import * 7 | from MOMDP import MOMDP, MOMDP_TOQ, MOMDP_TOQ_d 8 | 9 | 10 | def main(): 11 | load = 0 # 0 = compute policy and save policy object, 1 = load policy object, -1 = compute policy but DO NOT save it 12 | digitsResults = 6 # number of digits to print, just for visual output 13 | printLevel = 0 # 0 = only value function update and results, 1 = minimal, 2 = verbose 14 | 15 | # Evaluate single policy. Details in evaluateSinglePolicy() function 16 | discOpt = 4 17 | evaluateSinglePolicy(load, digitsResults, printLevel, discOpt) 18 | 19 | def evaluateSinglePolicy(load, digitsResults, printLevel, discOpt): 20 | numObst = 2 21 | unGoal = True 22 | 23 | numObst = 2 24 | unGoal = False 25 | 26 | if unGoal == False: 27 | gridWorld = '7x7' 28 | else: 29 | gridWorld = '7x7ug' 30 | # gridWorld = '5x5ug' 31 | policy = 'segway' 32 | momdpSegway = getMOMDP(gridWorld, numObst, policy, printLevel, load, discOpt, unGoal = unGoal) 33 | runSim(momdpSegway, gridWorld, numObst, policy, printLevel, digitsResults) 34 | 35 | policy = 'drone' 36 | if unGoal == False: 37 | gridWorld = '7x7_d' # IMPORTANT: for the drone there is always one goal --> unGoal = False 38 | else: 39 | gridWorld = '7x7ug_d' # IMPORTANT: for the drone there is always one goal --> unGoal = False 40 | gridWorld = '7x7ug_d' # IMPORTANT: for the drone there is always one goal --> unGoal = False 41 | momdpDrone = getMOMDP(gridWorld, numObst, policy, printLevel, load, discOpt, momdpSegway = momdpSegway) 42 | runSim(momdpDrone, gridWorld, numObst, policy, printLevel, digitsResults) 43 | 44 | def getMOMDP(gridWorld, numObst, policy, printLevel, load, discOpt, unGoal = False, momdpSegway = None): 45 | totTimeSteps, _, _ = loadParameters(gridWorld, numObst, unGoal) 46 | 47 | 48 | fileName = sys.path[0]+'/multiAgent/'+policy+'_'+gridWorld+'_'+str(numObst)+'.pkl' 49 | 50 | if load <= 0: # If load <= 0 compute the policy and store it if load == 0 51 | gridVar = loadGrid(gridWorld+'_'+str(numObst)) 52 | if policy == 'segway': 53 | momdp = MOMDP_TOQ(gridVar, totTimeSteps,printLevel, policy, discOpt, unGoal = unGoal) 54 | elif policy == 'drone': 55 | momdp = MOMDP_TOQ_d(gridVar, totTimeSteps,printLevel, policy, discOpt, momdpSegway) 56 | 57 | if load == 0: 58 | pickle_out = open(fileName,"wb") 59 | pickle.dump(momdp, pickle_out) 60 | pickle_out.close() 61 | else: 62 | pickle_in = open(fileName,"rb") 63 | momdp = pickle.load(pickle_in) 64 | 65 | return momdp 66 | 67 | def runSim(momdp, gridWorld, numObst, policy, printLevel, digits): 68 | totTimeSteps, loc, initBelief = loadParameters(gridWorld, numObst, momdp.unGoal) 69 | 70 | bt = [momdp.initBelief(initBelief)] # initial belief 71 | xt = [0] # initial state 72 | V_t0, Vrealized, J_t0, failure,tRun, xt, bt = eveluateMOMDP(momdp, loc, initBelief, xt, bt, printLevel) 73 | 74 | plotFun(momdp, xt, bt) 75 | print("cl: ", xt) 76 | 77 | if __name__ == "__main__": 78 | # execute only if run as a script 79 | main() -------------------------------------------------------------------------------- /launch/moving_obstacle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | [0.5, 5.5, 3.1415, 0.0, 0.0, 0.0, 0.0] 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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/StlSupport/details.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STL_DETAILS_H 12 | #define EIGEN_STL_DETAILS_H 13 | 14 | #ifndef EIGEN_ALIGNED_ALLOCATOR 15 | #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator 16 | #endif 17 | 18 | namespace Eigen { 19 | 20 | // This one is needed to prevent reimplementing the whole std::vector. 21 | template 22 | class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR 23 | { 24 | public: 25 | typedef std::size_t size_type; 26 | typedef std::ptrdiff_t difference_type; 27 | typedef T* pointer; 28 | typedef const T* const_pointer; 29 | typedef T& reference; 30 | typedef const T& const_reference; 31 | typedef T value_type; 32 | 33 | template 34 | struct rebind 35 | { 36 | typedef aligned_allocator_indirection other; 37 | }; 38 | 39 | aligned_allocator_indirection() {} 40 | aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR() {} 41 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 42 | template 43 | aligned_allocator_indirection(const aligned_allocator_indirection& ) {} 44 | template 45 | aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR& ) {} 46 | ~aligned_allocator_indirection() {} 47 | }; 48 | 49 | #if EIGEN_COMP_MSVC 50 | 51 | // sometimes, MSVC detects, at compile time, that the argument x 52 | // in std::vector::resize(size_t s,T x) won't be aligned and generate an error 53 | // even if this function is never called. Whence this little wrapper. 54 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \ 55 | typename Eigen::internal::conditional< \ 56 | Eigen::internal::is_arithmetic::value, \ 57 | T, \ 58 | Eigen::internal::workaround_msvc_stl_support \ 59 | >::type 60 | 61 | namespace internal { 62 | template struct workaround_msvc_stl_support : public T 63 | { 64 | inline workaround_msvc_stl_support() : T() {} 65 | inline workaround_msvc_stl_support(const T& other) : T(other) {} 66 | inline operator T& () { return *static_cast(this); } 67 | inline operator const T& () const { return *static_cast(this); } 68 | template 69 | inline T& operator=(const OtherT& other) 70 | { T::operator=(other); return *this; } 71 | inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other) 72 | { T::operator=(other); return *this; } 73 | }; 74 | } 75 | 76 | #else 77 | 78 | #define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T 79 | 80 | #endif 81 | 82 | } 83 | 84 | #endif // EIGEN_STL_DETAILS_H 85 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # segway_sim 2 | 3 | This is a ROS-based C++ simulation environment for a two-wheeled inverted pendulum (Segway) robot. It currently has two branches, a Model Predictive Control (MPC) branch that uses a Mixed Observable Markov Decision Process (MOMDP) planner for high-level planning, and uses the MPC for trajectory generation and control. The other branch is a Control Barrier Function (CBF) branch that guarantees that the system stays in a "safe" subset of the state space, regardless of the inputs given to the system from the user. 4 | 5 |

6 | 7 |

8 | 9 | ### Prerequisites 10 | 11 | Eigen is required. If Eigen is not installed, or you want to run the version of Eigen that runs on the Segway, use the Eigen_embedded folder and replace all instances of Eigen_embedded with Eigen. 12 | 13 | The OSQP solver is used for both branches, and can be found [here](https://github.com/oxfordcontrol/osqp) 14 | 15 | The MPC branch requires the following [library](https://github.com/urosolia/multirate-mpc-cbf) for MPC, as well as the following ROS [package](https://github.com/DrewSingletary/ambercortex_ros) for its message structure. 16 | 17 | CBF branch installation coming soon. 18 | 19 | ## Description 20 | 21 | ### The robot 22 | The simulation is based on the Ninebot Elite used in the AMBER lab [link](http://www.bipedalrobotics.com/). The code run in this simulation environment has been used directly on the hardware, as evidenced in this [![video](https://img.youtube.com/vi/Tr6bpjmzHcE/maxresdefault.jpg)](https://youtu.be/Tr6bpjmzHcE) 23 | 24 | ### How to run 25 | 26 | After intalling the MPC and OSQP libraries listend in the above Prerequisites section follows the following steps: 27 | 28 | - Run the main.py file in the folder /segway_sim/src/pyFun. It should save a .pkl object in the /segway_sim/src/pyFun/data/Qug_1 folder 29 | - Download the ambercorted_ros pakage from [here](http://www.bipedalrobotics.com/) 30 | - Checkout the dev branch in the ambercorted_ros package 31 | - Build your catkin workspace 32 | - Open the launch file main.launch. At the bottom of the launch file enter the folder where you would like to save your bag file 33 | - Run roslaunch segway_sim main.launch 34 | - In a new terminal run rosservice call /segway_sim/integrator/ui "cmd: 1" 35 | - After the simulation run the file readNewBag.py in the /segway_sim/plotUtils folder (it is needed to change the path where the bag file has been saved) 36 | 37 | 38 | ## Results 39 | Coming soon 40 | 41 | ## References 42 | 43 | This code is based on the following: 44 | 45 | (CBF) 46 | * Gurriet, T., Singletary, A., Reher, J., Ciarletta, L., Feron, E., & Ames, A. (2018, April). Towards a framework for realizable safety critical control through active set invariance. In 2018 ACM/IEEE 9th International Conference on Cyber-Physical Systems (ICCPS) (pp. 98-106). IEEE. [PDF](https://ieeexplore.ieee.org/abstract/document/8443725) 47 | * U. Rosolia and A. D. Ames, "Multi-Rate Control Design Leveraging Control Barrier Functions and Model Predictive Control Policies," in IEEE Control Systems Letters, vol. 5, no. 3, pp. 1007-1012, July 2021, [PDF](https://ieeexplore.ieee.org/document/9137248) 48 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/arch/NEON/MathFunctions.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | /* The sin, cos, exp, and log functions of this file come from 9 | * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/ 10 | */ 11 | 12 | #ifndef EIGEN_MATH_FUNCTIONS_NEON_H 13 | #define EIGEN_MATH_FUNCTIONS_NEON_H 14 | 15 | namespace Eigen { 16 | 17 | namespace internal { 18 | 19 | template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED 20 | Packet4f pexp(const Packet4f& _x) 21 | { 22 | Packet4f x = _x; 23 | Packet4f tmp, fx; 24 | 25 | _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f); 26 | _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f); 27 | _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f); 28 | _EIGEN_DECLARE_CONST_Packet4f(exp_hi, 88.3762626647950f); 29 | _EIGEN_DECLARE_CONST_Packet4f(exp_lo, -88.3762626647949f); 30 | _EIGEN_DECLARE_CONST_Packet4f(cephes_LOG2EF, 1.44269504088896341f); 31 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C1, 0.693359375f); 32 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C2, -2.12194440e-4f); 33 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p0, 1.9875691500E-4f); 34 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p1, 1.3981999507E-3f); 35 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p2, 8.3334519073E-3f); 36 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p3, 4.1665795894E-2f); 37 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f); 38 | _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f); 39 | 40 | x = vminq_f32(x, p4f_exp_hi); 41 | x = vmaxq_f32(x, p4f_exp_lo); 42 | 43 | /* express exp(x) as exp(g + n*log(2)) */ 44 | fx = vmlaq_f32(p4f_half, x, p4f_cephes_LOG2EF); 45 | 46 | /* perform a floorf */ 47 | tmp = vcvtq_f32_s32(vcvtq_s32_f32(fx)); 48 | 49 | /* if greater, substract 1 */ 50 | Packet4ui mask = vcgtq_f32(tmp, fx); 51 | mask = vandq_u32(mask, vreinterpretq_u32_f32(p4f_1)); 52 | 53 | fx = vsubq_f32(tmp, vreinterpretq_f32_u32(mask)); 54 | 55 | tmp = vmulq_f32(fx, p4f_cephes_exp_C1); 56 | Packet4f z = vmulq_f32(fx, p4f_cephes_exp_C2); 57 | x = vsubq_f32(x, tmp); 58 | x = vsubq_f32(x, z); 59 | 60 | Packet4f y = vmulq_f32(p4f_cephes_exp_p0, x); 61 | z = vmulq_f32(x, x); 62 | y = vaddq_f32(y, p4f_cephes_exp_p1); 63 | y = vmulq_f32(y, x); 64 | y = vaddq_f32(y, p4f_cephes_exp_p2); 65 | y = vmulq_f32(y, x); 66 | y = vaddq_f32(y, p4f_cephes_exp_p3); 67 | y = vmulq_f32(y, x); 68 | y = vaddq_f32(y, p4f_cephes_exp_p4); 69 | y = vmulq_f32(y, x); 70 | y = vaddq_f32(y, p4f_cephes_exp_p5); 71 | 72 | y = vmulq_f32(y, z); 73 | y = vaddq_f32(y, x); 74 | y = vaddq_f32(y, p4f_1); 75 | 76 | /* build 2^n */ 77 | int32x4_t mm; 78 | mm = vcvtq_s32_f32(fx); 79 | mm = vaddq_s32(mm, p4i_0x7f); 80 | mm = vshlq_n_s32(mm, 23); 81 | Packet4f pow2n = vreinterpretq_f32_s32(mm); 82 | 83 | y = vmulq_f32(y, pow2n); 84 | return y; 85 | } 86 | 87 | } // end namespace internal 88 | 89 | } // end namespace Eigen 90 | 91 | #endif // EIGEN_MATH_FUNCTIONS_NEON_H 92 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseLU/SparseLU_relax_snode.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | /* This file is a modified version of heap_relax_snode.c file in SuperLU 11 | * -- SuperLU routine (version 3.0) -- 12 | * Univ. of California Berkeley, Xerox Palo Alto Research Center, 13 | * and Lawrence Berkeley National Lab. 14 | * October 15, 2003 15 | * 16 | * Copyright (c) 1994 by Xerox Corporation. All rights reserved. 17 | * 18 | * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY 19 | * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. 20 | * 21 | * Permission is hereby granted to use or copy this program for any 22 | * purpose, provided the above notices are retained on all copies. 23 | * Permission to modify the code and to distribute modified code is 24 | * granted, provided the above notices are retained, and a notice that 25 | * the code was modified is included with the above copyright notice. 26 | */ 27 | 28 | #ifndef SPARSELU_RELAX_SNODE_H 29 | #define SPARSELU_RELAX_SNODE_H 30 | 31 | namespace Eigen { 32 | 33 | namespace internal { 34 | 35 | /** 36 | * \brief Identify the initial relaxed supernodes 37 | * 38 | * This routine is applied to a column elimination tree. 39 | * It assumes that the matrix has been reordered according to the postorder of the etree 40 | * \param n the number of columns 41 | * \param et elimination tree 42 | * \param relax_columns Maximum number of columns allowed in a relaxed snode 43 | * \param descendants Number of descendants of each node in the etree 44 | * \param relax_end last column in a supernode 45 | */ 46 | template 47 | void SparseLUImpl::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end) 48 | { 49 | 50 | // compute the number of descendants of each node in the etree 51 | Index parent; 52 | relax_end.setConstant(emptyIdxLU); 53 | descendants.setZero(); 54 | for (Index j = 0; j < n; j++) 55 | { 56 | parent = et(j); 57 | if (parent != n) // not the dummy root 58 | descendants(parent) += descendants(j) + 1; 59 | } 60 | // Identify the relaxed supernodes by postorder traversal of the etree 61 | Index snode_start; // beginning of a snode 62 | for (Index j = 0; j < n; ) 63 | { 64 | parent = et(j); 65 | snode_start = j; 66 | while ( parent != n && descendants(parent) < relax_columns ) 67 | { 68 | j = parent; 69 | parent = et(j); 70 | } 71 | // Found a supernode in postordered etree, j is the last column 72 | relax_end(snode_start) = StorageIndex(j); // Record last column 73 | j++; 74 | // Search for a new leaf 75 | while (descendants(j) != 0 && j < n) j++; 76 | } // End postorder traversal of the etree 77 | 78 | } 79 | 80 | } // end namespace internal 81 | 82 | } // end namespace Eigen 83 | #endif 84 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/misc/Image.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MISC_IMAGE_H 11 | #define EIGEN_MISC_IMAGE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | /** \class image_retval_base 18 | * 19 | */ 20 | template 21 | struct traits > 22 | { 23 | typedef typename DecompositionType::MatrixType MatrixType; 24 | typedef Matrix< 25 | typename MatrixType::Scalar, 26 | MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose 27 | // dimension is the number of rows of the original matrix 28 | Dynamic, // we don't know at compile time the dimension of the image (the rank) 29 | MatrixType::Options, 30 | MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix, 31 | MatrixType::MaxColsAtCompileTime // so it has the same number of rows and at most as many columns. 32 | > ReturnType; 33 | }; 34 | 35 | template struct image_retval_base 36 | : public ReturnByValue > 37 | { 38 | typedef _DecompositionType DecompositionType; 39 | typedef typename DecompositionType::MatrixType MatrixType; 40 | typedef ReturnByValue Base; 41 | 42 | image_retval_base(const DecompositionType& dec, const MatrixType& originalMatrix) 43 | : m_dec(dec), m_rank(dec.rank()), 44 | m_cols(m_rank == 0 ? 1 : m_rank), 45 | m_originalMatrix(originalMatrix) 46 | {} 47 | 48 | inline Index rows() const { return m_dec.rows(); } 49 | inline Index cols() const { return m_cols; } 50 | inline Index rank() const { return m_rank; } 51 | inline const DecompositionType& dec() const { return m_dec; } 52 | inline const MatrixType& originalMatrix() const { return m_originalMatrix; } 53 | 54 | template inline void evalTo(Dest& dst) const 55 | { 56 | static_cast*>(this)->evalTo(dst); 57 | } 58 | 59 | protected: 60 | const DecompositionType& m_dec; 61 | Index m_rank, m_cols; 62 | const MatrixType& m_originalMatrix; 63 | }; 64 | 65 | } // end namespace internal 66 | 67 | #define EIGEN_MAKE_IMAGE_HELPERS(DecompositionType) \ 68 | typedef typename DecompositionType::MatrixType MatrixType; \ 69 | typedef typename MatrixType::Scalar Scalar; \ 70 | typedef typename MatrixType::RealScalar RealScalar; \ 71 | typedef Eigen::internal::image_retval_base Base; \ 72 | using Base::dec; \ 73 | using Base::originalMatrix; \ 74 | using Base::rank; \ 75 | using Base::rows; \ 76 | using Base::cols; \ 77 | image_retval(const DecompositionType& dec, const MatrixType& originalMatrix) \ 78 | : Base(dec, originalMatrix) {} 79 | 80 | } // end namespace Eigen 81 | 82 | #endif // EIGEN_MISC_IMAGE_H 83 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/QR/HouseholderQR_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | 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 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * Householder QR decomposition of a matrix w/o pivoting based on 30 | * LAPACKE_?geqrf function. 31 | ******************************************************************************** 32 | */ 33 | 34 | #ifndef EIGEN_QR_LAPACKE_H 35 | #define EIGEN_QR_LAPACKE_H 36 | 37 | namespace Eigen { 38 | 39 | namespace internal { 40 | 41 | /** \internal Specialization for the data types supported by LAPACKe */ 42 | 43 | #define EIGEN_LAPACKE_QR_NOPIV(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX) \ 44 | template \ 45 | struct householder_qr_inplace_blocked \ 46 | { \ 47 | static void run(MatrixQR& mat, HCoeffs& hCoeffs, Index = 32, \ 48 | typename MatrixQR::Scalar* = 0) \ 49 | { \ 50 | lapack_int m = (lapack_int) mat.rows(); \ 51 | lapack_int n = (lapack_int) mat.cols(); \ 52 | lapack_int lda = (lapack_int) mat.outerStride(); \ 53 | lapack_int matrix_order = (MatrixQR::IsRowMajor) ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ 54 | LAPACKE_##LAPACKE_PREFIX##geqrf( matrix_order, m, n, (LAPACKE_TYPE*)mat.data(), lda, (LAPACKE_TYPE*)hCoeffs.data()); \ 55 | hCoeffs.adjointInPlace(); \ 56 | } \ 57 | }; 58 | 59 | EIGEN_LAPACKE_QR_NOPIV(double, double, d) 60 | EIGEN_LAPACKE_QR_NOPIV(float, float, s) 61 | EIGEN_LAPACKE_QR_NOPIV(dcomplex, lapack_complex_double, z) 62 | EIGEN_LAPACKE_QR_NOPIV(scomplex, lapack_complex_float, c) 63 | 64 | } // end namespace internal 65 | 66 | } // end namespace Eigen 67 | 68 | #endif // EIGEN_QR_LAPACKE_H 69 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/plugins/MatrixCwiseUnaryOps.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | // This file is included into the body of the base classes supporting matrix specific coefficient-wise functions. 12 | // This include MatrixBase and SparseMatrixBase. 13 | 14 | 15 | typedef CwiseUnaryOp, const Derived> CwiseAbsReturnType; 16 | typedef CwiseUnaryOp, const Derived> CwiseAbs2ReturnType; 17 | typedef CwiseUnaryOp, const Derived> CwiseSqrtReturnType; 18 | typedef CwiseUnaryOp, const Derived> CwiseSignReturnType; 19 | typedef CwiseUnaryOp, const Derived> CwiseInverseReturnType; 20 | 21 | /// \returns an expression of the coefficient-wise absolute value of \c *this 22 | /// 23 | /// Example: \include MatrixBase_cwiseAbs.cpp 24 | /// Output: \verbinclude MatrixBase_cwiseAbs.out 25 | /// 26 | EIGEN_DOC_UNARY_ADDONS(cwiseAbs,absolute value) 27 | /// 28 | /// \sa cwiseAbs2() 29 | /// 30 | EIGEN_DEVICE_FUNC 31 | EIGEN_STRONG_INLINE const CwiseAbsReturnType 32 | cwiseAbs() const { return CwiseAbsReturnType(derived()); } 33 | 34 | /// \returns an expression of the coefficient-wise squared absolute value of \c *this 35 | /// 36 | /// Example: \include MatrixBase_cwiseAbs2.cpp 37 | /// Output: \verbinclude MatrixBase_cwiseAbs2.out 38 | /// 39 | EIGEN_DOC_UNARY_ADDONS(cwiseAbs2,squared absolute value) 40 | /// 41 | /// \sa cwiseAbs() 42 | /// 43 | EIGEN_DEVICE_FUNC 44 | EIGEN_STRONG_INLINE const CwiseAbs2ReturnType 45 | cwiseAbs2() const { return CwiseAbs2ReturnType(derived()); } 46 | 47 | /// \returns an expression of the coefficient-wise square root of *this. 48 | /// 49 | /// Example: \include MatrixBase_cwiseSqrt.cpp 50 | /// Output: \verbinclude MatrixBase_cwiseSqrt.out 51 | /// 52 | EIGEN_DOC_UNARY_ADDONS(cwiseSqrt,square-root) 53 | /// 54 | /// \sa cwisePow(), cwiseSquare() 55 | /// 56 | EIGEN_DEVICE_FUNC 57 | inline const CwiseSqrtReturnType 58 | cwiseSqrt() const { return CwiseSqrtReturnType(derived()); } 59 | 60 | /// \returns an expression of the coefficient-wise signum of *this. 61 | /// 62 | /// Example: \include MatrixBase_cwiseSign.cpp 63 | /// Output: \verbinclude MatrixBase_cwiseSign.out 64 | /// 65 | EIGEN_DOC_UNARY_ADDONS(cwiseSign,sign function) 66 | /// 67 | EIGEN_DEVICE_FUNC 68 | inline const CwiseSignReturnType 69 | cwiseSign() const { return CwiseSignReturnType(derived()); } 70 | 71 | 72 | /// \returns an expression of the coefficient-wise inverse of *this. 73 | /// 74 | /// Example: \include MatrixBase_cwiseInverse.cpp 75 | /// Output: \verbinclude MatrixBase_cwiseInverse.out 76 | /// 77 | EIGEN_DOC_UNARY_ADDONS(cwiseInverse,inverse) 78 | /// 79 | /// \sa cwiseProduct() 80 | /// 81 | EIGEN_DEVICE_FUNC 82 | inline const CwiseInverseReturnType 83 | cwiseInverse() const { return CwiseInverseReturnType(derived()); } 84 | 85 | 86 | -------------------------------------------------------------------------------- /launch/main.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | [0.5, 7.5, 3.14, 0.0, 0.0, 0.0, 0.0] 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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseCore/SparseDot.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_DOT_H 11 | #define EIGEN_SPARSE_DOT_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | typename internal::traits::Scalar 18 | SparseMatrixBase::dot(const MatrixBase& other) const 19 | { 20 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 21 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) 22 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) 23 | EIGEN_STATIC_ASSERT((internal::is_same::value), 24 | YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 25 | 26 | eigen_assert(size() == other.size()); 27 | eigen_assert(other.size()>0 && "you are using a non initialized vector"); 28 | 29 | internal::evaluator thisEval(derived()); 30 | typename internal::evaluator::InnerIterator i(thisEval, 0); 31 | Scalar res(0); 32 | while (i) 33 | { 34 | res += numext::conj(i.value()) * other.coeff(i.index()); 35 | ++i; 36 | } 37 | return res; 38 | } 39 | 40 | template 41 | template 42 | typename internal::traits::Scalar 43 | SparseMatrixBase::dot(const SparseMatrixBase& other) const 44 | { 45 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) 46 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived) 47 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived) 48 | EIGEN_STATIC_ASSERT((internal::is_same::value), 49 | YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 50 | 51 | eigen_assert(size() == other.size()); 52 | 53 | internal::evaluator thisEval(derived()); 54 | typename internal::evaluator::InnerIterator i(thisEval, 0); 55 | 56 | internal::evaluator otherEval(other.derived()); 57 | typename internal::evaluator::InnerIterator j(otherEval, 0); 58 | 59 | Scalar res(0); 60 | while (i && j) 61 | { 62 | if (i.index()==j.index()) 63 | { 64 | res += numext::conj(i.value()) * j.value(); 65 | ++i; ++j; 66 | } 67 | else if (i.index() 76 | inline typename NumTraits::Scalar>::Real 77 | SparseMatrixBase::squaredNorm() const 78 | { 79 | return numext::real((*this).cwiseAbs2().sum()); 80 | } 81 | 82 | template 83 | inline typename NumTraits::Scalar>::Real 84 | SparseMatrixBase::norm() const 85 | { 86 | using std::sqrt; 87 | return sqrt(squaredNorm()); 88 | } 89 | 90 | template 91 | inline typename NumTraits::Scalar>::Real 92 | SparseMatrixBase::blueNorm() const 93 | { 94 | return internal::blueNorm_impl(*this); 95 | } 96 | } // end namespace Eigen 97 | 98 | #endif // EIGEN_SPARSE_DOT_H 99 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseCore/SparseTranspose.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2015 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSETRANSPOSE_H 11 | #define EIGEN_SPARSETRANSPOSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | template 17 | class SparseTransposeImpl 18 | : public SparseMatrixBase > 19 | {}; 20 | 21 | template 22 | class SparseTransposeImpl 23 | : public SparseCompressedBase > 24 | { 25 | typedef SparseCompressedBase > Base; 26 | public: 27 | using Base::derived; 28 | typedef typename Base::Scalar Scalar; 29 | typedef typename Base::StorageIndex StorageIndex; 30 | 31 | inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); } 32 | 33 | inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); } 34 | inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); } 35 | inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); } 36 | inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); } 37 | 38 | inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); } 39 | inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); } 40 | inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); } 41 | inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); } 42 | }; 43 | } 44 | 45 | template class TransposeImpl 46 | : public internal::SparseTransposeImpl 47 | { 48 | protected: 49 | typedef internal::SparseTransposeImpl Base; 50 | }; 51 | 52 | namespace internal { 53 | 54 | template 55 | struct unary_evaluator, IteratorBased> 56 | : public evaluator_base > 57 | { 58 | typedef typename evaluator::InnerIterator EvalIterator; 59 | public: 60 | typedef Transpose XprType; 61 | 62 | inline Index nonZerosEstimate() const { 63 | return m_argImpl.nonZerosEstimate(); 64 | } 65 | 66 | class InnerIterator : public EvalIterator 67 | { 68 | public: 69 | EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer) 70 | : EvalIterator(unaryOp.m_argImpl,outer) 71 | {} 72 | 73 | Index row() const { return EvalIterator::col(); } 74 | Index col() const { return EvalIterator::row(); } 75 | }; 76 | 77 | enum { 78 | CoeffReadCost = evaluator::CoeffReadCost, 79 | Flags = XprType::Flags 80 | }; 81 | 82 | explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {} 83 | 84 | protected: 85 | evaluator m_argImpl; 86 | }; 87 | 88 | } // end namespace internal 89 | 90 | } // end namespace Eigen 91 | 92 | #endif // EIGEN_SPARSETRANSPOSE_H 93 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/LU/Determinant.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Benoit Jacob 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_DETERMINANT_H 11 | #define EIGEN_DETERMINANT_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | template 18 | inline const typename Derived::Scalar bruteforce_det3_helper 19 | (const MatrixBase& matrix, int a, int b, int c) 20 | { 21 | return matrix.coeff(0,a) 22 | * (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b)); 23 | } 24 | 25 | template 26 | const typename Derived::Scalar bruteforce_det4_helper 27 | (const MatrixBase& matrix, int j, int k, int m, int n) 28 | { 29 | return (matrix.coeff(j,0) * matrix.coeff(k,1) - matrix.coeff(k,0) * matrix.coeff(j,1)) 30 | * (matrix.coeff(m,2) * matrix.coeff(n,3) - matrix.coeff(n,2) * matrix.coeff(m,3)); 31 | } 32 | 33 | template struct determinant_impl 36 | { 37 | static inline typename traits::Scalar run(const Derived& m) 38 | { 39 | if(Derived::ColsAtCompileTime==Dynamic && m.rows()==0) 40 | return typename traits::Scalar(1); 41 | return m.partialPivLu().determinant(); 42 | } 43 | }; 44 | 45 | template struct determinant_impl 46 | { 47 | static inline typename traits::Scalar run(const Derived& m) 48 | { 49 | return m.coeff(0,0); 50 | } 51 | }; 52 | 53 | template struct determinant_impl 54 | { 55 | static inline typename traits::Scalar run(const Derived& m) 56 | { 57 | return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1); 58 | } 59 | }; 60 | 61 | template struct determinant_impl 62 | { 63 | static inline typename traits::Scalar run(const Derived& m) 64 | { 65 | return bruteforce_det3_helper(m,0,1,2) 66 | - bruteforce_det3_helper(m,1,0,2) 67 | + bruteforce_det3_helper(m,2,0,1); 68 | } 69 | }; 70 | 71 | template struct determinant_impl 72 | { 73 | static typename traits::Scalar run(const Derived& m) 74 | { 75 | // trick by Martin Costabel to compute 4x4 det with only 30 muls 76 | return bruteforce_det4_helper(m,0,1,2,3) 77 | - bruteforce_det4_helper(m,0,2,1,3) 78 | + bruteforce_det4_helper(m,0,3,1,2) 79 | + bruteforce_det4_helper(m,1,2,0,3) 80 | - bruteforce_det4_helper(m,1,3,0,2) 81 | + bruteforce_det4_helper(m,2,3,0,1); 82 | } 83 | }; 84 | 85 | } // end namespace internal 86 | 87 | /** \lu_module 88 | * 89 | * \returns the determinant of this matrix 90 | */ 91 | template 92 | inline typename internal::traits::Scalar MatrixBase::determinant() const 93 | { 94 | eigen_assert(rows() == cols()); 95 | typedef typename internal::nested_eval::type Nested; 96 | return internal::determinant_impl::type>::run(derived()); 97 | } 98 | 99 | } // end namespace Eigen 100 | 101 | #endif // EIGEN_DETERMINANT_H 102 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/VectorBlock.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_VECTORBLOCK_H 12 | #define EIGEN_VECTORBLOCK_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | template 18 | struct traits > 19 | : public traits::Flags & RowMajorBit ? 1 : Size, 21 | traits::Flags & RowMajorBit ? Size : 1> > 22 | { 23 | }; 24 | } 25 | 26 | /** \class VectorBlock 27 | * \ingroup Core_Module 28 | * 29 | * \brief Expression of a fixed-size or dynamic-size sub-vector 30 | * 31 | * \tparam VectorType the type of the object in which we are taking a sub-vector 32 | * \tparam Size size of the sub-vector we are taking at compile time (optional) 33 | * 34 | * This class represents an expression of either a fixed-size or dynamic-size sub-vector. 35 | * It is the return type of DenseBase::segment(Index,Index) and DenseBase::segment(Index) and 36 | * most of the time this is the only way it is used. 37 | * 38 | * However, if you want to directly maniputate sub-vector expressions, 39 | * for instance if you want to write a function returning such an expression, you 40 | * will need to use this class. 41 | * 42 | * Here is an example illustrating the dynamic case: 43 | * \include class_VectorBlock.cpp 44 | * Output: \verbinclude class_VectorBlock.out 45 | * 46 | * \note Even though this expression has dynamic size, in the case where \a VectorType 47 | * has fixed size, this expression inherits a fixed maximal size which means that evaluating 48 | * it does not cause a dynamic memory allocation. 49 | * 50 | * Here is an example illustrating the fixed-size case: 51 | * \include class_FixedVectorBlock.cpp 52 | * Output: \verbinclude class_FixedVectorBlock.out 53 | * 54 | * \sa class Block, DenseBase::segment(Index,Index,Index,Index), DenseBase::segment(Index,Index) 55 | */ 56 | template class VectorBlock 57 | : public Block::Flags & RowMajorBit ? 1 : Size, 59 | internal::traits::Flags & RowMajorBit ? Size : 1> 60 | { 61 | typedef Block::Flags & RowMajorBit ? 1 : Size, 63 | internal::traits::Flags & RowMajorBit ? Size : 1> Base; 64 | enum { 65 | IsColVector = !(internal::traits::Flags & RowMajorBit) 66 | }; 67 | public: 68 | EIGEN_DENSE_PUBLIC_INTERFACE(VectorBlock) 69 | 70 | using Base::operator=; 71 | 72 | /** Dynamic-size constructor 73 | */ 74 | EIGEN_DEVICE_FUNC 75 | inline VectorBlock(VectorType& vector, Index start, Index size) 76 | : Base(vector, 77 | IsColVector ? start : 0, IsColVector ? 0 : start, 78 | IsColVector ? size : 1, IsColVector ? 1 : size) 79 | { 80 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock); 81 | } 82 | 83 | /** Fixed-size constructor 84 | */ 85 | EIGEN_DEVICE_FUNC 86 | inline VectorBlock(VectorType& vector, Index start) 87 | : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start) 88 | { 89 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock); 90 | } 91 | }; 92 | 93 | 94 | } // end namespace Eigen 95 | 96 | #endif // EIGEN_VECTORBLOCK_H 97 | -------------------------------------------------------------------------------- /src/manual_teleop_node.cpp: -------------------------------------------------------------------------------- 1 | #include "segway_sim/manual_teleop_node.hpp" 2 | 3 | using namespace Eigen; 4 | 5 | ros::NodeHandle *nh_; 6 | ros::NodeHandle *nhParams_; 7 | ros::Publisher pub_cmd_; 8 | 9 | segway_sim::cmd cmd_; 10 | 11 | uint32_t iter_; 12 | double hz_; 13 | double vx_, vyaw_, boost_; 14 | std::string input_dev_; 15 | bool run_; 16 | double smoothing_dt_; 17 | Vector2d cmdVec_(0.0,0.0); 18 | 19 | void my_handler(int sig) 20 | { 21 | ROS_INFO("Received SIGINT signal"); 22 | run_ = false; 23 | } 24 | 25 | int main (int argc, char *argv[]) 26 | { 27 | // Init ros 28 | ros::init(argc,argv,"planner"); 29 | 30 | // Instanciate NodeHandles 31 | nhParams_ = new ros::NodeHandle("~"); 32 | nh_ = new ros::NodeHandle(); 33 | 34 | // Initialize some variables 35 | iter_ = 0; 36 | run_ = true; 37 | cmd_.cmd.resize(2,0.0); 38 | int ch; 39 | struct termios oldt; 40 | struct termios newt; 41 | 42 | // Remove terminal echo 43 | tcgetattr(STDIN_FILENO, &oldt); 44 | newt = oldt; 45 | newt.c_lflag &= ~ECHO; 46 | tcsetattr(fileno(stdin), TCSANOW, &newt); 47 | 48 | // Init pubs, subs and srvs 49 | pub_cmd_ = nh_->advertise("cmd", 1); 50 | 51 | // Retreive params 52 | nhParams_->param("input_dev",input_dev_,"/dev/input/event0"); 53 | nhParams_->param("vx",vx_,1.0); 54 | nhParams_->param("vyaw",vyaw_,1.0); 55 | nhParams_->param("boost",boost_,2.0); 56 | nhParams_->param("smoothing_dt",smoothing_dt_,0.0); 57 | nhParams_->param("hz",hz_,100.0); 58 | 59 | //Initialize keyboard observer 60 | cKeyboard kb(input_dev_.c_str()); 61 | if(!kb.active) 62 | { 63 | ROS_INFO("Could not find keyboard at: %s",input_dev_.c_str()); 64 | } 65 | 66 | // Display node info 67 | ROS_INFO("Manual teleop node successfuly started with:"); 68 | ROS_INFO("___input_dev=%s",input_dev_.c_str()); 69 | 70 | ROS_INFO("___hz=%.0f",hz_); 71 | ROS_INFO("___vx=%.2f",vx_); 72 | ROS_INFO("___vyaw=%.2f",vyaw_); 73 | ROS_INFO("___boost=%.2f",boost_); 74 | ROS_INFO("___smoothing_dt=%.3f",smoothing_dt_); 75 | 76 | // Initialize rate 77 | ros::Rate rate(hz_); 78 | 79 | // Initialize SIGINT handler 80 | signal(SIGINT,my_handler); 81 | 82 | // Take it for a spin 83 | while(run_) 84 | { 85 | //Get latest parameters 86 | nhParams_->param("vx",vx_,1.0); 87 | nhParams_->param("vyaw",vyaw_,1.0); 88 | nhParams_->param("boost",boost_,2.0); 89 | nhParams_->param("smoothing_dt",smoothing_dt_,0.0); 90 | 91 | // Compute command 92 | Vector2d cmdVec(0.0,0.0); 93 | 94 | double boost = 1.0; 95 | if(kb.getKeyState(KEY_LEFTSHIFT)) 96 | boost = boost_; 97 | 98 | if(kb.getKeyState(KEY_W)) 99 | cmdVec+=boost*vx_*moveBindings['w']; 100 | 101 | if(kb.getKeyState(KEY_S)) 102 | cmdVec+=boost*vx_*moveBindings['s']; 103 | 104 | if(kb.getKeyState(KEY_A)) 105 | cmdVec+=boost*vyaw_*moveBindings['a']; 106 | 107 | if(kb.getKeyState(KEY_D)) 108 | cmdVec+=boost*vyaw_*moveBindings['d']; 109 | 110 | cmd_.header.seq = iter_; 111 | cmd_.header.stamp = ros::Time::now(); 112 | cmd_.status = static_cast(STATUS::RUNNING); 113 | 114 | if(smoothing_dt_>=0.001) 115 | { 116 | cmdVec_ += (1/hz_)*(cmdVec-cmdVec_)/(smoothing_dt_/3.0); 117 | } 118 | else 119 | cmdVec_ = cmdVec; 120 | 121 | cmd_.cmd[0] = cmdVec_(0); 122 | cmd_.cmd[1] = cmdVec_(1); 123 | iter_++; 124 | 125 | // Publish cmd message 126 | pub_cmd_.publish(cmd_); 127 | 128 | //Wait for tick 129 | rate.sleep(); 130 | } 131 | 132 | // Send last command as 0 133 | cmd_.cmd[0] = 0.0; 134 | cmd_.cmd[1] = 0.0; 135 | 136 | pub_cmd_.publish(cmd_); 137 | 138 | // Reapply terminal settings 139 | tcsetattr(STDIN_FILENO, TCSANOW, &oldt); 140 | 141 | ros::shutdown(); 142 | 143 | return 0; 144 | } -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/util/DisableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_WARNINGS_DISABLED 2 | #define EIGEN_WARNINGS_DISABLED 3 | 4 | #ifdef _MSC_VER 5 | // 4100 - unreferenced formal parameter (occurred e.g. in aligned_allocator::destroy(pointer p)) 6 | // 4101 - unreferenced local variable 7 | // 4127 - conditional expression is constant 8 | // 4181 - qualifier applied to reference type ignored 9 | // 4211 - nonstandard extension used : redefined extern to static 10 | // 4244 - 'argument' : conversion from 'type1' to 'type2', possible loss of data 11 | // 4273 - QtAlignedMalloc, inconsistent DLL linkage 12 | // 4324 - structure was padded due to declspec(align()) 13 | // 4503 - decorated name length exceeded, name was truncated 14 | // 4512 - assignment operator could not be generated 15 | // 4522 - 'class' : multiple assignment operators specified 16 | // 4700 - uninitialized local variable 'xyz' used 17 | // 4714 - function marked as __forceinline not inlined 18 | // 4717 - 'function' : recursive on all control paths, function will cause runtime stack overflow 19 | // 4800 - 'type' : forcing value to bool 'true' or 'false' (performance warning) 20 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 21 | #pragma warning( push ) 22 | #endif 23 | #pragma warning( disable : 4100 4101 4127 4181 4211 4244 4273 4324 4503 4512 4522 4700 4714 4717 4800) 24 | 25 | #elif defined __INTEL_COMPILER 26 | // 2196 - routine is both "inline" and "noinline" ("noinline" assumed) 27 | // ICC 12 generates this warning even without any inline keyword, when defining class methods 'inline' i.e. inside of class body 28 | // typedef that may be a reference type. 29 | // 279 - controlling expression is constant 30 | // ICC 12 generates this warning on assert(constant_expression_depending_on_template_params) and frankly this is a legitimate use case. 31 | // 1684 - conversion from pointer to same-sized integral type (potential portability problem) 32 | // 2259 - non-pointer conversion from "Eigen::Index={ptrdiff_t={long}}" to "int" may lose significant bits 33 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 34 | #pragma warning push 35 | #endif 36 | #pragma warning disable 2196 279 1684 2259 37 | 38 | #elif defined __clang__ 39 | // -Wconstant-logical-operand - warning: use of logical && with constant operand; switch to bitwise & or remove constant 40 | // this is really a stupid warning as it warns on compile-time expressions involving enums 41 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 42 | #pragma clang diagnostic push 43 | #endif 44 | #pragma clang diagnostic ignored "-Wconstant-logical-operand" 45 | 46 | #elif defined __GNUC__ && __GNUC__>=6 47 | 48 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 49 | #pragma GCC diagnostic push 50 | #endif 51 | #pragma GCC diagnostic ignored "-Wignored-attributes" 52 | 53 | #endif 54 | 55 | #if defined __NVCC__ 56 | // Disable the "statement is unreachable" message 57 | #pragma diag_suppress code_is_unreachable 58 | // Disable the "dynamic initialization in unreachable code" message 59 | #pragma diag_suppress initialization_not_reachable 60 | // Disable the "invalid error number" message that we get with older versions of nvcc 61 | #pragma diag_suppress 1222 62 | // Disable the "calling a __host__ function from a __host__ __device__ function is not allowed" messages (yes, there are many of them and they seem to change with every version of the compiler) 63 | #pragma diag_suppress 2527 64 | #pragma diag_suppress 2529 65 | #pragma diag_suppress 2651 66 | #pragma diag_suppress 2653 67 | #pragma diag_suppress 2668 68 | #pragma diag_suppress 2669 69 | #pragma diag_suppress 2670 70 | #pragma diag_suppress 2671 71 | #pragma diag_suppress 2735 72 | #pragma diag_suppress 2737 73 | #endif 74 | 75 | #endif // not EIGEN_WARNINGS_DISABLED 76 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/LU/PartialPivLU_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | 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 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * LU decomposition with partial pivoting based on LAPACKE_?getrf function. 30 | ******************************************************************************** 31 | */ 32 | 33 | #ifndef EIGEN_PARTIALLU_LAPACK_H 34 | #define EIGEN_PARTIALLU_LAPACK_H 35 | 36 | namespace Eigen { 37 | 38 | namespace internal { 39 | 40 | /** \internal Specialization for the data types supported by LAPACKe */ 41 | 42 | #define EIGEN_LAPACKE_LU_PARTPIV(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX) \ 43 | template \ 44 | struct partial_lu_impl \ 45 | { \ 46 | /* \internal performs the LU decomposition in-place of the matrix represented */ \ 47 | static lapack_int blocked_lu(Index rows, Index cols, EIGTYPE* lu_data, Index luStride, lapack_int* row_transpositions, lapack_int& nb_transpositions, lapack_int maxBlockSize=256) \ 48 | { \ 49 | EIGEN_UNUSED_VARIABLE(maxBlockSize);\ 50 | lapack_int matrix_order, first_zero_pivot; \ 51 | lapack_int m, n, lda, *ipiv, info; \ 52 | EIGTYPE* a; \ 53 | /* Set up parameters for ?getrf */ \ 54 | matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \ 55 | lda = convert_index(luStride); \ 56 | a = lu_data; \ 57 | ipiv = row_transpositions; \ 58 | m = convert_index(rows); \ 59 | n = convert_index(cols); \ 60 | nb_transpositions = 0; \ 61 | \ 62 | info = LAPACKE_##LAPACKE_PREFIX##getrf( matrix_order, m, n, (LAPACKE_TYPE*)a, lda, ipiv ); \ 63 | \ 64 | for(int i=0;i= 0); \ 67 | /* something should be done with nb_transpositions */ \ 68 | \ 69 | first_zero_pivot = info; \ 70 | return first_zero_pivot; \ 71 | } \ 72 | }; 73 | 74 | EIGEN_LAPACKE_LU_PARTPIV(double, double, d) 75 | EIGEN_LAPACKE_LU_PARTPIV(float, float, s) 76 | EIGEN_LAPACKE_LU_PARTPIV(dcomplex, lapack_complex_double, z) 77 | EIGEN_LAPACKE_LU_PARTPIV(scomplex, lapack_complex_float, c) 78 | 79 | } // end namespace internal 80 | 81 | } // end namespace Eigen 82 | 83 | #endif // EIGEN_PARTIALLU_LAPACK_H 84 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/MathFunctionsImpl.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com) 5 | // Copyright (C) 2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_MATHFUNCTIONSIMPL_H 12 | #define EIGEN_MATHFUNCTIONSIMPL_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | /** \internal \returns the hyperbolic tan of \a a (coeff-wise) 19 | Doesn't do anything fancy, just a 13/6-degree rational interpolant which 20 | is accurate up to a couple of ulp in the range [-9, 9], outside of which 21 | the tanh(x) = +/-1. 22 | 23 | This implementation works on both scalars and packets. 24 | */ 25 | template 26 | T generic_fast_tanh_float(const T& a_x) 27 | { 28 | // Clamp the inputs to the range [-9, 9] since anything outside 29 | // this range is +/-1.0f in single-precision. 30 | const T plus_9 = pset1(9.f); 31 | const T minus_9 = pset1(-9.f); 32 | // NOTE GCC prior to 6.3 might improperly optimize this max/min 33 | // step such that if a_x is nan, x will be either 9 or -9, 34 | // and tanh will return 1 or -1 instead of nan. 35 | // This is supposed to be fixed in gcc6.3, 36 | // see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867 37 | const T x = pmax(minus_9,pmin(plus_9,a_x)); 38 | // The monomial coefficients of the numerator polynomial (odd). 39 | const T alpha_1 = pset1(4.89352455891786e-03f); 40 | const T alpha_3 = pset1(6.37261928875436e-04f); 41 | const T alpha_5 = pset1(1.48572235717979e-05f); 42 | const T alpha_7 = pset1(5.12229709037114e-08f); 43 | const T alpha_9 = pset1(-8.60467152213735e-11f); 44 | const T alpha_11 = pset1(2.00018790482477e-13f); 45 | const T alpha_13 = pset1(-2.76076847742355e-16f); 46 | 47 | // The monomial coefficients of the denominator polynomial (even). 48 | const T beta_0 = pset1(4.89352518554385e-03f); 49 | const T beta_2 = pset1(2.26843463243900e-03f); 50 | const T beta_4 = pset1(1.18534705686654e-04f); 51 | const T beta_6 = pset1(1.19825839466702e-06f); 52 | 53 | // Since the polynomials are odd/even, we need x^2. 54 | const T x2 = pmul(x, x); 55 | 56 | // Evaluate the numerator polynomial p. 57 | T p = pmadd(x2, alpha_13, alpha_11); 58 | p = pmadd(x2, p, alpha_9); 59 | p = pmadd(x2, p, alpha_7); 60 | p = pmadd(x2, p, alpha_5); 61 | p = pmadd(x2, p, alpha_3); 62 | p = pmadd(x2, p, alpha_1); 63 | p = pmul(x, p); 64 | 65 | // Evaluate the denominator polynomial p. 66 | T q = pmadd(x2, beta_6, beta_4); 67 | q = pmadd(x2, q, beta_2); 68 | q = pmadd(x2, q, beta_0); 69 | 70 | // Divide the numerator by the denominator. 71 | return pdiv(p, q); 72 | } 73 | 74 | template 75 | EIGEN_STRONG_INLINE 76 | RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y) 77 | { 78 | EIGEN_USING_STD_MATH(sqrt); 79 | RealScalar p, qp; 80 | p = numext::maxi(x,y); 81 | if(p==RealScalar(0)) return RealScalar(0); 82 | qp = numext::mini(y,x) / p; 83 | return p * sqrt(RealScalar(1) + qp*qp); 84 | } 85 | 86 | template 87 | struct hypot_impl 88 | { 89 | typedef typename NumTraits::Real RealScalar; 90 | static inline RealScalar run(const Scalar& x, const Scalar& y) 91 | { 92 | EIGEN_USING_STD_MATH(abs); 93 | return positive_real_hypot(abs(x), abs(y)); 94 | } 95 | }; 96 | 97 | } // end namespace internal 98 | 99 | } // end namespace Eigen 100 | 101 | #endif // EIGEN_MATHFUNCTIONSIMPL_H 102 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Eigenvalues/RealSchur_LAPACKE.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | 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 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | ******************************************************************************** 28 | * Content : Eigen bindings to LAPACKe 29 | * Real Schur needed to real unsymmetrical eigenvalues/eigenvectors. 30 | ******************************************************************************** 31 | */ 32 | 33 | #ifndef EIGEN_REAL_SCHUR_LAPACKE_H 34 | #define EIGEN_REAL_SCHUR_LAPACKE_H 35 | 36 | namespace Eigen { 37 | 38 | /** \internal Specialization for the data types supported by LAPACKe */ 39 | 40 | #define EIGEN_LAPACKE_SCHUR_REAL(EIGTYPE, LAPACKE_TYPE, LAPACKE_PREFIX, LAPACKE_PREFIX_U, EIGCOLROW, LAPACKE_COLROW) \ 41 | template<> template inline \ 42 | RealSchur >& \ 43 | RealSchur >::compute(const EigenBase& matrix, bool computeU) \ 44 | { \ 45 | eigen_assert(matrix.cols() == matrix.rows()); \ 46 | \ 47 | lapack_int n = internal::convert_index(matrix.cols()), sdim, info; \ 48 | lapack_int matrix_order = LAPACKE_COLROW; \ 49 | char jobvs, sort='N'; \ 50 | LAPACK_##LAPACKE_PREFIX_U##_SELECT2 select = 0; \ 51 | jobvs = (computeU) ? 'V' : 'N'; \ 52 | m_matU.resize(n, n); \ 53 | lapack_int ldvs = internal::convert_index(m_matU.outerStride()); \ 54 | m_matT = matrix; \ 55 | lapack_int lda = internal::convert_index(m_matT.outerStride()); \ 56 | Matrix wr, wi; \ 57 | wr.resize(n, 1); wi.resize(n, 1); \ 58 | info = LAPACKE_##LAPACKE_PREFIX##gees( matrix_order, jobvs, sort, select, n, (LAPACKE_TYPE*)m_matT.data(), lda, &sdim, (LAPACKE_TYPE*)wr.data(), (LAPACKE_TYPE*)wi.data(), (LAPACKE_TYPE*)m_matU.data(), ldvs ); \ 59 | if(info == 0) \ 60 | m_info = Success; \ 61 | else \ 62 | m_info = NoConvergence; \ 63 | \ 64 | m_isInitialized = true; \ 65 | m_matUisUptodate = computeU; \ 66 | return *this; \ 67 | \ 68 | } 69 | 70 | EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, ColMajor, LAPACK_COL_MAJOR) 71 | EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, ColMajor, LAPACK_COL_MAJOR) 72 | EIGEN_LAPACKE_SCHUR_REAL(double, double, d, D, RowMajor, LAPACK_ROW_MAJOR) 73 | EIGEN_LAPACKE_SCHUR_REAL(float, float, s, S, RowMajor, LAPACK_ROW_MAJOR) 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_REAL_SCHUR_LAPACKE_H 78 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/NestByValue.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_NESTBYVALUE_H 12 | #define EIGEN_NESTBYVALUE_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | template 18 | struct traits > : public traits 19 | {}; 20 | } 21 | 22 | /** \class NestByValue 23 | * \ingroup Core_Module 24 | * 25 | * \brief Expression which must be nested by value 26 | * 27 | * \tparam ExpressionType the type of the object of which we are requiring nesting-by-value 28 | * 29 | * This class is the return type of MatrixBase::nestByValue() 30 | * and most of the time this is the only way it is used. 31 | * 32 | * \sa MatrixBase::nestByValue() 33 | */ 34 | template class NestByValue 35 | : public internal::dense_xpr_base< NestByValue >::type 36 | { 37 | public: 38 | 39 | typedef typename internal::dense_xpr_base::type Base; 40 | EIGEN_DENSE_PUBLIC_INTERFACE(NestByValue) 41 | 42 | EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {} 43 | 44 | EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); } 45 | EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); } 46 | EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); } 47 | EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); } 48 | 49 | EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const 50 | { 51 | return m_expression.coeff(row, col); 52 | } 53 | 54 | EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col) 55 | { 56 | return m_expression.const_cast_derived().coeffRef(row, col); 57 | } 58 | 59 | EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const 60 | { 61 | return m_expression.coeff(index); 62 | } 63 | 64 | EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index) 65 | { 66 | return m_expression.const_cast_derived().coeffRef(index); 67 | } 68 | 69 | template 70 | inline const PacketScalar packet(Index row, Index col) const 71 | { 72 | return m_expression.template packet(row, col); 73 | } 74 | 75 | template 76 | inline void writePacket(Index row, Index col, const PacketScalar& x) 77 | { 78 | m_expression.const_cast_derived().template writePacket(row, col, x); 79 | } 80 | 81 | template 82 | inline const PacketScalar packet(Index index) const 83 | { 84 | return m_expression.template packet(index); 85 | } 86 | 87 | template 88 | inline void writePacket(Index index, const PacketScalar& x) 89 | { 90 | m_expression.const_cast_derived().template writePacket(index, x); 91 | } 92 | 93 | EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; } 94 | 95 | protected: 96 | const ExpressionType m_expression; 97 | }; 98 | 99 | /** \returns an expression of the temporary version of *this. 100 | */ 101 | template 102 | inline const NestByValue 103 | DenseBase::nestByValue() const 104 | { 105 | return NestByValue(derived()); 106 | } 107 | 108 | } // end namespace Eigen 109 | 110 | #endif // EIGEN_NESTBYVALUE_H 111 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/Inverse.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_INVERSE_H 11 | #define EIGEN_INVERSE_H 12 | 13 | namespace Eigen { 14 | 15 | template class InverseImpl; 16 | 17 | namespace internal { 18 | 19 | template 20 | struct traits > 21 | : traits 22 | { 23 | typedef typename XprType::PlainObject PlainObject; 24 | typedef traits BaseTraits; 25 | enum { 26 | Flags = BaseTraits::Flags & RowMajorBit 27 | }; 28 | }; 29 | 30 | } // end namespace internal 31 | 32 | /** \class Inverse 33 | * 34 | * \brief Expression of the inverse of another expression 35 | * 36 | * \tparam XprType the type of the expression we are taking the inverse 37 | * 38 | * This class represents an abstract expression of A.inverse() 39 | * and most of the time this is the only way it is used. 40 | * 41 | */ 42 | template 43 | class Inverse : public InverseImpl::StorageKind> 44 | { 45 | public: 46 | typedef typename XprType::StorageIndex StorageIndex; 47 | typedef typename XprType::PlainObject PlainObject; 48 | typedef typename XprType::Scalar Scalar; 49 | typedef typename internal::ref_selector::type XprTypeNested; 50 | typedef typename internal::remove_all::type XprTypeNestedCleaned; 51 | typedef typename internal::ref_selector::type Nested; 52 | typedef typename internal::remove_all::type NestedExpression; 53 | 54 | explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr) 55 | : m_xpr(xpr) 56 | {} 57 | 58 | EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); } 59 | EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); } 60 | 61 | EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; } 62 | 63 | protected: 64 | XprTypeNested m_xpr; 65 | }; 66 | 67 | // Generic API dispatcher 68 | template 69 | class InverseImpl 70 | : public internal::generic_xpr_base >::type 71 | { 72 | public: 73 | typedef typename internal::generic_xpr_base >::type Base; 74 | typedef typename XprType::Scalar Scalar; 75 | private: 76 | 77 | Scalar coeff(Index row, Index col) const; 78 | Scalar coeff(Index i) const; 79 | }; 80 | 81 | namespace internal { 82 | 83 | /** \internal 84 | * \brief Default evaluator for Inverse expression. 85 | * 86 | * This default evaluator for Inverse expression simply evaluate the inverse into a temporary 87 | * by a call to internal::call_assignment_no_alias. 88 | * Therefore, inverse implementers only have to specialize Assignment, ...> for 89 | * there own nested expression. 90 | * 91 | * \sa class Inverse 92 | */ 93 | template 94 | struct unary_evaluator > 95 | : public evaluator::PlainObject> 96 | { 97 | typedef Inverse InverseType; 98 | typedef typename InverseType::PlainObject PlainObject; 99 | typedef evaluator Base; 100 | 101 | enum { Flags = Base::Flags | EvalBeforeNestingBit }; 102 | 103 | unary_evaluator(const InverseType& inv_xpr) 104 | : m_result(inv_xpr.rows(), inv_xpr.cols()) 105 | { 106 | ::new (static_cast(this)) Base(m_result); 107 | internal::call_assignment_no_alias(m_result, inv_xpr); 108 | } 109 | 110 | protected: 111 | PlainObject m_result; 112 | }; 113 | 114 | } // end namespace internal 115 | 116 | } // end namespace Eigen 117 | 118 | #endif // EIGEN_INVERSE_H 119 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Core/NoAlias.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_NOALIAS_H 11 | #define EIGEN_NOALIAS_H 12 | 13 | namespace Eigen { 14 | 15 | /** \class NoAlias 16 | * \ingroup Core_Module 17 | * 18 | * \brief Pseudo expression providing an operator = assuming no aliasing 19 | * 20 | * \tparam ExpressionType the type of the object on which to do the lazy assignment 21 | * 22 | * This class represents an expression with special assignment operators 23 | * assuming no aliasing between the target expression and the source expression. 24 | * More precisely it alloas to bypass the EvalBeforeAssignBit flag of the source expression. 25 | * It is the return type of MatrixBase::noalias() 26 | * and most of the time this is the only way it is used. 27 | * 28 | * \sa MatrixBase::noalias() 29 | */ 30 | template class StorageBase> 31 | class NoAlias 32 | { 33 | public: 34 | typedef typename ExpressionType::Scalar Scalar; 35 | 36 | explicit NoAlias(ExpressionType& expression) : m_expression(expression) {} 37 | 38 | template 39 | EIGEN_DEVICE_FUNC 40 | EIGEN_STRONG_INLINE ExpressionType& operator=(const StorageBase& other) 41 | { 42 | call_assignment_no_alias(m_expression, other.derived(), internal::assign_op()); 43 | return m_expression; 44 | } 45 | 46 | template 47 | EIGEN_DEVICE_FUNC 48 | EIGEN_STRONG_INLINE ExpressionType& operator+=(const StorageBase& other) 49 | { 50 | call_assignment_no_alias(m_expression, other.derived(), internal::add_assign_op()); 51 | return m_expression; 52 | } 53 | 54 | template 55 | EIGEN_DEVICE_FUNC 56 | EIGEN_STRONG_INLINE ExpressionType& operator-=(const StorageBase& other) 57 | { 58 | call_assignment_no_alias(m_expression, other.derived(), internal::sub_assign_op()); 59 | return m_expression; 60 | } 61 | 62 | EIGEN_DEVICE_FUNC 63 | ExpressionType& expression() const 64 | { 65 | return m_expression; 66 | } 67 | 68 | protected: 69 | ExpressionType& m_expression; 70 | }; 71 | 72 | /** \returns a pseudo expression of \c *this with an operator= assuming 73 | * no aliasing between \c *this and the source expression. 74 | * 75 | * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag. 76 | * Currently, even though several expressions may alias, only product 77 | * expressions have this flag. Therefore, noalias() is only usefull when 78 | * the source expression contains a matrix product. 79 | * 80 | * Here are some examples where noalias is usefull: 81 | * \code 82 | * D.noalias() = A * B; 83 | * D.noalias() += A.transpose() * B; 84 | * D.noalias() -= 2 * A * B.adjoint(); 85 | * \endcode 86 | * 87 | * On the other hand the following example will lead to a \b wrong result: 88 | * \code 89 | * A.noalias() = A * B; 90 | * \endcode 91 | * because the result matrix A is also an operand of the matrix product. Therefore, 92 | * there is no alternative than evaluating A * B in a temporary, that is the default 93 | * behavior when you write: 94 | * \code 95 | * A = A * B; 96 | * \endcode 97 | * 98 | * \sa class NoAlias 99 | */ 100 | template 101 | NoAlias MatrixBase::noalias() 102 | { 103 | return NoAlias(derived()); 104 | } 105 | 106 | } // end namespace Eigen 107 | 108 | #endif // EIGEN_NOALIAS_H 109 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/Geometry/EulerAngles.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_EULERANGLES_H 11 | #define EIGEN_EULERANGLES_H 12 | 13 | namespace Eigen { 14 | 15 | /** \geometry_module \ingroup Geometry_Module 16 | * 17 | * 18 | * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2) 19 | * 20 | * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}. 21 | * For instance, in: 22 | * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode 23 | * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that 24 | * we have the following equality: 25 | * \code 26 | * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) 27 | * * AngleAxisf(ea[1], Vector3f::UnitX()) 28 | * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode 29 | * This corresponds to the right-multiply conventions (with right hand side frames). 30 | * 31 | * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. 32 | * 33 | * \sa class AngleAxis 34 | */ 35 | template 36 | EIGEN_DEVICE_FUNC inline Matrix::Scalar,3,1> 37 | MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const 38 | { 39 | EIGEN_USING_STD_MATH(atan2) 40 | EIGEN_USING_STD_MATH(sin) 41 | EIGEN_USING_STD_MATH(cos) 42 | /* Implemented from Graphics Gems IV */ 43 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) 44 | 45 | Matrix res; 46 | typedef Matrix Vector2; 47 | 48 | const Index odd = ((a0+1)%3 == a1) ? 0 : 1; 49 | const Index i = a0; 50 | const Index j = (a0 + 1 + odd)%3; 51 | const Index k = (a0 + 2 - odd)%3; 52 | 53 | if (a0==a2) 54 | { 55 | res[0] = atan2(coeff(j,i), coeff(k,i)); 56 | if((odd && res[0]Scalar(0))) 57 | { 58 | if(res[0] > Scalar(0)) { 59 | res[0] -= Scalar(EIGEN_PI); 60 | } 61 | else { 62 | res[0] += Scalar(EIGEN_PI); 63 | } 64 | Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 65 | res[1] = -atan2(s2, coeff(i,i)); 66 | } 67 | else 68 | { 69 | Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); 70 | res[1] = atan2(s2, coeff(i,i)); 71 | } 72 | 73 | // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, 74 | // we can compute their respective rotation, and apply its inverse to M. Since the result must 75 | // be a rotation around x, we have: 76 | // 77 | // c2 s1.s2 c1.s2 1 0 0 78 | // 0 c1 -s1 * M = 0 c3 s3 79 | // -s2 s1.c2 c1.c2 0 -s3 c3 80 | // 81 | // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 82 | 83 | Scalar s1 = sin(res[0]); 84 | Scalar c1 = cos(res[0]); 85 | res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); 86 | } 87 | else 88 | { 89 | res[0] = atan2(coeff(j,k), coeff(k,k)); 90 | Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm(); 91 | if((odd && res[0]Scalar(0))) { 92 | if(res[0] > Scalar(0)) { 93 | res[0] -= Scalar(EIGEN_PI); 94 | } 95 | else { 96 | res[0] += Scalar(EIGEN_PI); 97 | } 98 | res[1] = atan2(-coeff(i,k), -c2); 99 | } 100 | else 101 | res[1] = atan2(-coeff(i,k), c2); 102 | Scalar s1 = sin(res[0]); 103 | Scalar c1 = cos(res[0]); 104 | res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); 105 | } 106 | if (!odd) 107 | res = -res; 108 | 109 | return res; 110 | } 111 | 112 | } // end namespace Eigen 113 | 114 | #endif // EIGEN_EULERANGLES_H 115 | -------------------------------------------------------------------------------- /include/Eigen_embedded/src/SparseLU/SparseLU_copy_to_ucol.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | /* 10 | 11 | * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU 12 | 13 | * -- SuperLU routine (version 2.0) -- 14 | * Univ. of California Berkeley, Xerox Palo Alto Research Center, 15 | * and Lawrence Berkeley National Lab. 16 | * November 15, 1997 17 | * 18 | * Copyright (c) 1994 by Xerox Corporation. All rights reserved. 19 | * 20 | * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY 21 | * EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. 22 | * 23 | * Permission is hereby granted to use or copy this program for any 24 | * purpose, provided the above notices are retained on all copies. 25 | * Permission to modify the code and to distribute modified code is 26 | * granted, provided the above notices are retained, and a notice that 27 | * the code was modified is included with the above copyright notice. 28 | */ 29 | #ifndef SPARSELU_COPY_TO_UCOL_H 30 | #define SPARSELU_COPY_TO_UCOL_H 31 | 32 | namespace Eigen { 33 | namespace internal { 34 | 35 | /** 36 | * \brief Performs numeric block updates (sup-col) in topological order 37 | * 38 | * \param jcol current column to update 39 | * \param nseg Number of segments in the U part 40 | * \param segrep segment representative ... 41 | * \param repfnz First nonzero column in each row ... 42 | * \param perm_r Row permutation 43 | * \param dense Store the full representation of the column 44 | * \param glu Global LU data. 45 | * \return 0 - successful return 46 | * > 0 - number of bytes allocated when run out of space 47 | * 48 | */ 49 | template 50 | Index SparseLUImpl::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, 51 | BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu) 52 | { 53 | Index ksub, krep, ksupno; 54 | 55 | Index jsupno = glu.supno(jcol); 56 | 57 | // For each nonzero supernode segment of U[*,j] in topological order 58 | Index k = nseg - 1, i; 59 | StorageIndex nextu = glu.xusub(jcol); 60 | Index kfnz, isub, segsize; 61 | Index new_next,irow; 62 | Index fsupc, mem; 63 | for (ksub = 0; ksub < nseg; ksub++) 64 | { 65 | krep = segrep(k); k--; 66 | ksupno = glu.supno(krep); 67 | if (jsupno != ksupno ) // should go into ucol(); 68 | { 69 | kfnz = repfnz(krep); 70 | if (kfnz != emptyIdxLU) 71 | { // Nonzero U-segment 72 | fsupc = glu.xsup(ksupno); 73 | isub = glu.xlsub(fsupc) + kfnz - fsupc; 74 | segsize = krep - kfnz + 1; 75 | new_next = nextu + segsize; 76 | while (new_next > glu.nzumax) 77 | { 78 | mem = memXpand(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions); 79 | if (mem) return mem; 80 | mem = memXpand(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions); 81 | if (mem) return mem; 82 | 83 | } 84 | 85 | for (i = 0; i < segsize; i++) 86 | { 87 | irow = glu.lsub(isub); 88 | glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order 89 | glu.ucol(nextu) = dense(irow); 90 | dense(irow) = Scalar(0.0); 91 | nextu++; 92 | isub++; 93 | } 94 | 95 | } // end nonzero U-segment 96 | 97 | } // end if jsupno 98 | 99 | } // end for each segment 100 | glu.xusub(jcol + 1) = nextu; // close U(*,jcol) 101 | return 0; 102 | } 103 | 104 | } // namespace internal 105 | } // end namespace Eigen 106 | 107 | #endif // SPARSELU_COPY_TO_UCOL_H 108 | --------------------------------------------------------------------------------