├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── action └── TrainModel.action ├── cfg ├── adaptiveClbf.cfg └── figureEight.cfg ├── launch ├── controller_adaptiveclbf.launch ├── encoder_odometry.launch ├── figure_eight.launch └── model_service.launch ├── msg └── DebugData.msg ├── package.xml ├── rviz ├── racecar_sinusoid.rviz ├── rviz_hw.rviz └── rviz_plotting.rviz ├── scripts ├── barrier_formulas.m ├── cleanup.sh ├── rqt_multiplot.xml ├── set_physics_props.sh ├── startup_base.yaml ├── startup_hw.yaml └── startup_sim.yaml ├── src ├── ALPaCA │ ├── README.md │ ├── __init__.py │ ├── alpaca.py │ ├── dataViz.py │ └── dataset.py ├── adaptive_clbf.py ├── adaptive_clbf_node.py ├── bag_data.py ├── cbf.py ├── dyn_reconfig_server.py ├── dynamics.py ├── encoder_odometry.py ├── figure_eight.py ├── lyapunov.py ├── model_gp.py ├── model_service.py ├── plot_bagged_data.py ├── qp_solver.py ├── scaledgp.py ├── test_adaptive_clbf.py └── vanilla_nn.py └── srv ├── AddData2Model.srv └── PredictModel.srv /.gitignore: -------------------------------------------------------------------------------- 1 | ../src/adaptive_clbf.pyc 2 | ../src/cbf.pyc 3 | ../src/dynamics.pyc 4 | ../src/lyapunov.pyc 5 | ../src/model_gp.pyc 6 | ../src/qp_solver.pyc 7 | ../src/scaledgp.pyc 8 | ../src/ALPaCA/alpaca.pyc 9 | ../src/ALPaCA/dataset.pyc 10 | ../src/ALPaCA/__init__.pyc 11 | .gitignore 12 | src/adaptive_clbf.pyc 13 | src/cbf.pyc 14 | src/dynamics.pyc 15 | src/lyapunov.pyc 16 | src/model_gp.pyc 17 | src/qp_solver.pyc 18 | src/scaledgp.pyc 19 | src/vanilla_nn.pyc 20 | model_service.pyc 21 | summaries/ 22 | ALPaCA/alpaca.pyc 23 | ALPaCA/dataset.pyc 24 | ALPaCA/__init__.pyc 25 | src/ALPaCA/alpaca.pyc 26 | src/ALPaCA/dataset.pyc 27 | src/ALPaCA/__init__.pyc 28 | alpaca.pyc 29 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(controller_adaptiveclbf) 3 | 4 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp rospy std_msgs actionlib_msgs nav_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | DebugData.msg 9 | ) 10 | 11 | add_service_files(DIRECTORY srv FILES AddData2Model.srv PredictModel.srv) 12 | add_action_files(DIRECTORY action FILES TrainModel.action) 13 | 14 | generate_messages( 15 | DEPENDENCIES 16 | std_msgs 17 | actionlib_msgs 18 | nav_msgs 19 | ) 20 | 21 | generate_dynamic_reconfigure_options( 22 | cfg/adaptiveClbf.cfg 23 | cfg/figureEight.cfg 24 | ) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS dynamic_reconfigure message_runtime roscpp rospy std_msgs 28 | ) 29 | 30 | install(PROGRAMS src/adaptive_clbf_node.py src/figure_eight.py src/encoder_odometry.py src/model_service.py src/dyn_reconfig_server.py src/bag_data.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 31 | 32 | install(DIRECTORY launch 33 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 34 | ) 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # BALSA: BAyesian Learning-based Safety and Adaptation 3 | 4 | Fan, David D., Jennifer Nguyen, Rohan Thakker, Nikhilesh Alatur, Ali-akbar Agha-mohammadi, and Evangelos A. Theodorou. "Bayesian Learning-Based Adaptive Control for Safety Critical Systems." [arXiv preprint arXiv:1910.02325](https://arxiv.org/abs/1910.02325) (2019). 5 | 6 | ### Paper Abstract 7 | Deep learning has enjoyed much recent success, and applying state-of-the-art model learning methods to controls is an exciting prospect. However, there is a strong reluctance to use these methods on safety-critical systems, which have constraints on safety, stability, and real-time performance. We propose a framework which satisfies these constraints while allowing the use of deep neural networks for learning model uncertainties. Central to our method is the use of Bayesian model learning, which provides an avenue for maintaining appropriate degrees of caution in the face of the unknown. In the proposed approach, we develop an adaptive control framework leveraging the theory of stochastic CLFs (Control Lypunov Functions) and stochastic CBFs (Control Barrier Functions) along with tractable Bayesian model learning via Gaussian Processes or Bayesian neural networks. Under reasonable assumptions, we guarantee stability and safety while adapting to unknown dynamics with probability 1. We demonstrate this architecture for high-speed terrestrial mobility targeting potential applications in safety-critical high-speed Mars rover missions. 8 | 9 | ## Getting started 10 | The `master` branch contains code for running the controller on hardware, as well as replicating the toy dynamics plots from the paper. The `sim_plots` branch contains code for running the gazebo simulation. 11 | 12 | ### Prerequisites 13 | The following packages can be installed with `pip`: 14 | * `numpy` 15 | * `scipy` 16 | * `progress` 17 | * `osqp` 18 | * `GPy` 19 | * `tensorflow` 20 | * `keras` 21 | * `matplotlib` 22 | 23 | You will also need ROS `melodic` and `gazebo`. Also `rqt_multiplot_plugin` is used for plotting. You will also need `tmux` and `tmuxp` for running the start-up scripts. 24 | 25 | ### Plotting 26 | To replicate the plots from the paper, run: 27 | 28 | ``` 29 | ./src/test_adaptive_clbf.py 30 | ``` 31 | 32 | ### Simulation 33 | To run the simulation (and obtain plots), first checkout the `sim_plots` branch. Then run: 34 | ``` 35 | tmuxp load scripts/startup_sim.yaml 36 | ``` 37 | 38 | ### Hardware 39 | For the on-board computer, run 40 | 41 | ``` 42 | tmuxp load scripts/startup_hw.yaml 43 | ``` 44 | 45 | and for the base station computer, run 46 | 47 | ``` 48 | tmuxp load scripts/startup_base.yaml 49 | ``` 50 | 51 | More documentation for getting hardware running (ros topics, hardware prequisites, etc.) will be added in the future. 52 | 53 | 54 | ## Citation 55 | ``` 56 | @article{fan2019bayesian, 57 | title={Bayesian Learning-Based Adaptive Control for Safety Critical Systems}, 58 | author={Fan, David D and Nguyen, Jennifer and Thakker, Rohan and Alatur, Nikhilesh and Agha-mohammadi, Ali-akbar and Theodorou, Evangelos A}, 59 | journal={arXiv preprint arXiv:1910.02325}, 60 | year={2019} 61 | } 62 | ``` 63 | 64 | ## Licensing 65 | The code in this project is licensed under GNU General Public license v3.0. 66 | 67 | -------------------------------------------------------------------------------- /action/TrainModel.action: -------------------------------------------------------------------------------- 1 | # goal definition 2 | --- 3 | # result definition 4 | bool model_trained 5 | --- 6 | # feedback definition 7 | -------------------------------------------------------------------------------- /cfg/adaptiveClbf.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='controller_adaptiveclbf' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | # Name Type Level Description Default Min Max 11 | gen.add("enable", bool_t, 0, "Enable node operation.", True) 12 | 13 | vehicle = gen.add_group("Vehicle") 14 | vehicle.add("vehicle_length", double_t, 0, "Vehicle Length (m)", 0.4, 0.01,5.0) 15 | vehicle.add("steering_limit", double_t, 0, "Steering Limit (?)", 0.5, 0.0,2.0) 16 | vehicle.add("scale_acceleration", double_t, 0, "Desired velocity gain times output acceleration.", 0.0, 0.0, 100.0) 17 | vehicle.add("acceleration_deadband", double_t, 0, "Current control deadband.", 3.0, 0.0, 100.0) 18 | vehicle.add("acceleration_gain", double_t, 0, "Desired acceleration gain to current.", 8.0, 0.0, 100.0) 19 | vehicle.add("max_accel", double_t, 0, "Max Acceleration (N)", 4.0, -50.0,50.0) 20 | vehicle.add("min_accel", double_t, 0, "Min Acceleration (N)", -4.0, -50.0,50.0) 21 | vehicle.add("rate", double_t, 0, "Controller Rate (Hz)", 50.0, 0.1,100.0) 22 | 23 | learning = gen.add_group("Learning") 24 | learning.add("learning_verbose", bool_t, 0, "Enable Learning verbose mode", False) 25 | learning.add("use_model", bool_t, 0, "Use learned model in adaptive controller", False) 26 | learning.add("add_data", bool_t, 0, "Whether or not to record data for training", True) 27 | learning.add("check_model", bool_t, 0, "Compute training loss online", True) 28 | learning.add("model_train", bool_t, 0, "Train model in adaptive controller", True) 29 | learning.add("measurement_noise", double_t, 0, "Prior on noise for model, how much to trust model", 40.0, 0.0,100.0) 30 | learning.add("N_data", int_t, 0, "Number of datapoints to save", 50000, 1, 100000) 31 | learning.add("N_updates", int_t, 0, "Number of training updates", 50, 1, 50000) 32 | learning.add("meta_batch_size", int_t, 0, "Meta-batch size", 50, 1, 1000) 33 | learning.add("data_horizon", int_t, 0, "Number of context data points", 20, 1, 1000) 34 | learning.add("test_horizon", int_t, 0, "Number of test data points", 30, 1, 1000) 35 | learning.add("min_datapoints", int_t, 0, "Minimum number of data points before training", 500, 1, 100000) 36 | learning.add("save_data_interval", int_t, 0, "Interval at which to save data", 10000, 1, 10000000) 37 | learning.add("learning_rate", double_t, 0, "Learning rate", 0.001, 0, 10) 38 | 39 | control = gen.add_group("Control") 40 | control.add("reverse_velocity_goal", bool_t, 0, "Reverse desired velocity if vehicle stops.", False) 41 | control.add("use_qp", bool_t, 0, "Use qp to optimize for controls", True) 42 | control.add("desired_vel", double_t, 0, "Desired velocity at goal", 0.0, -100.0, 100.0) 43 | control.add("verbose", bool_t, 0, "Enable Controller Verbose mode", False) 44 | control.add("kp_z", double_t, 0, "Position Gain", 1.0, 0.0,100.0) 45 | control.add("kd_z", double_t, 0, "Velocity Gain", 1.0, 0.0,100.0) 46 | control.add("max_error", double_t, 0, "clip pd error", 10.0,0.0,1000.0) 47 | control.add("kp_goal", double_t, 0, "Set desired velocity proportional to distance to goal (<0 to disable)", -1.0, -1.0,10.0) 48 | control.add("clf_epsilon", double_t, 0, "CLF Epsilon Convergence Factor", 100.0, 0.001,10000.0) 49 | control.add("qp_u_cost", double_t, 0, "QP Controls Cost", 100.0, 0.0,1.0e10) 50 | control.add("qp_u_prev_cost", double_t, 0, "QP Controls Smoothing Cost", 0.0, 0.0,1.0e10) 51 | control.add("qp_p1_cost", double_t, 0, "QP Lyapunov Slack Cost", 1.0, 0.0,1.0e12) 52 | control.add("qp_p2_cost", double_t, 0, "QP Barrier Slack Cost", 1.0e12, 0.0,1.0e12) 53 | control.add("qp_ksig", double_t, 0, "QP k_sigma Safety factor", 1.0, 0.0, 1.0e10) 54 | control.add("qp_max_var", double_t, 0, "QP Max Variance for sigDelta", 1.0, 0.0, 100.0) 55 | control.add("qp_verbose", bool_t, 0, "Enable QP Verbose mode", False) 56 | 57 | barriers = gen.add_group("Barriers") 58 | barriers.add("use_barrier_vel", bool_t, 0, "CBF for Velocity Enable/Disable", True) 59 | barriers.add("max_velocity", double_t, 0, "CBF Barrier for Max Velocity", 5.0, -100.0,100.0) 60 | barriers.add("min_velocity", double_t, 0, "CBF Barrier for Min Velocity", 0.0, -100.0,100.0) 61 | barriers.add("barrier_vel_gamma", double_t, 0, "CBF Barrier gamma for Velocity", 100.0, 0.0,10000.0) 62 | barriers.add("use_barrier_pointcloud", bool_t, 0, "CBF for Points on pointcloud Enable/Disable", False) 63 | barriers.add("barrier_radius", double_t, 0, "CBF Barrier for pointcloud, min radius", 0.4, -10.0,100.0) 64 | barriers.add("barrier_radius_velocity_scale", double_t, 0, "CBF Barrier for pointcloud, scale radius by velocity", 0.2, -10.0,100.0) 65 | barriers.add("barrier_pc_gamma_p", double_t, 0, "CBF Barrier for pointcloud, gamma_p", 1.0, 0.0,10000.0) 66 | barriers.add("barrier_pc_gamma", double_t, 0, "CBF Barrier for pointcloud, gamma", 10.0, 0.0,10000.0) 67 | barriers.add("barrier_max_distance", double_t, 0, "CBF Barrier max distance", 3.0, 0.0,100.0) 68 | barriers.add("barrier_resolution", double_t, 0, "CBF Barrier resolution", 0.1, 0.0,10.0) 69 | 70 | 71 | exit(gen.generate(PACKAGE, "controller_adaptiveclbf", "adaptiveClbf")) 72 | -------------------------------------------------------------------------------- /cfg/figureEight.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='controller_adaptiveclbf' 4 | import roslib 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | # Name Type Level Description Default Min Max 11 | gen.add("enable", bool_t, 0, "Enable node operation.", False) 12 | 13 | gen.add("width", double_t, 0, "Figure 8 width (m)", 5.0, 0.0,100.0) 14 | gen.add("height", double_t, 0, "Figure 8 height (m)", 5.0, 0.0,100.0) 15 | gen.add("speed", double_t, 0, "Figure 8 speed (m/s)", 1.5, 0.01,100.0) 16 | gen.add("freq", double_t, 0, "Oscillation frequency", 0.25, 0.001,100.0) 17 | gen.add("x_offset", double_t, 0, "Figure 8 center x(m)", 7.0, -1000.0,1000.0) 18 | gen.add("y_offset", double_t, 0, "Figure 8 center y(m)", 0.0, -1000.0,1000.0) 19 | 20 | exit(gen.generate(PACKAGE, "controller_adaptiveclbf", "figureEight")) 21 | -------------------------------------------------------------------------------- /launch/controller_adaptiveclbf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/encoder_odometry.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/figure_eight.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/model_service.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /msg/DebugData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 latency 3 | float64 dt 4 | float64 heading 5 | float64 vel_x_body 6 | float64 vel_y_body 7 | float64[] z 8 | float64[] z_ref 9 | float64[] z_dot 10 | float64[] y_out 11 | float64[] mu_rm 12 | float64[] mu_pd 13 | float64[] mu_ad 14 | float64[] mu_model 15 | float64[] rho 16 | float64[] mu_qp 17 | float64[] mu 18 | float64[] u_new 19 | float64[] u_unsat 20 | float64[] trueDelta 21 | float64 true_predict_error 22 | float64[] mDelta 23 | float64[] sigDelta 24 | nav_msgs/Odometry odom -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | controller_adaptiveclbf 3 | 0.0.1 4 | 5 | Controller for Ackermann steered vehicles which adapts to model uncertainty and maintains safety and stability 6 | 7 | David D. Fan 8 | David D. Fan 9 | BSD 10 | git@bitbucket.org:ddfan/controller_adaptiveclbf.git 11 | 12 | catkin 13 | message_generation 14 | actionlib_msgs 15 | message_runtime 16 | actionlib_msgs 17 | 18 | dynamic_reconfigure 19 | roscpp 20 | rospy 21 | std_msgs 22 | nav_msgs 23 | ackermann_msgs 24 | 25 | 26 | -------------------------------------------------------------------------------- /rviz/racecar_sinusoid.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | - /Goal1 12 | - /Goal1/Shape1 13 | - /Reference1 14 | - /Reference1/Shape1 15 | - /Reference1/Covariance1 16 | - /Reference1/Covariance1/Position1 17 | Splitter Ratio: 0.5 18 | Tree Height: 717 19 | - Class: rviz/Selection 20 | Name: Selection 21 | - Class: rviz/Tool Properties 22 | Expanded: 23 | - /2D Pose Estimate1 24 | - /2D Nav Goal1 25 | - /Publish Point1 26 | Name: Tool Properties 27 | Splitter Ratio: 0.5886790156364441 28 | - Class: rviz/Views 29 | Expanded: 30 | - /Current View1 31 | Name: Views 32 | Splitter Ratio: 0.5 33 | - Class: rviz/Time 34 | Experimental: false 35 | Name: Time 36 | SyncMode: 0 37 | SyncSource: LaserScan 38 | Preferences: 39 | PromptSaveOnExit: true 40 | Toolbars: 41 | toolButtonStyle: 2 42 | Visualization Manager: 43 | Class: "" 44 | Displays: 45 | - Alpha: 0.5 46 | Cell Size: 1 47 | Class: rviz/Grid 48 | Color: 160; 160; 164 49 | Enabled: true 50 | Line Style: 51 | Line Width: 0.029999999329447746 52 | Value: Lines 53 | Name: Grid 54 | Normal Cell Count: 0 55 | Offset: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Plane: XY 60 | Plane Cell Count: 200 61 | Reference Frame: map 62 | Value: true 63 | - Alpha: 1 64 | Class: rviz/RobotModel 65 | Collision Enabled: false 66 | Enabled: true 67 | Links: 68 | All Links Enabled: true 69 | Expand Joint Details: false 70 | Expand Link Details: false 71 | Expand Tree: false 72 | Link Tree Style: Links in Alphabetic Order 73 | Name: RobotModel 74 | Robot Description: robot_description 75 | TF Prefix: "" 76 | Update Interval: 0 77 | Value: true 78 | Visual Enabled: true 79 | - Alpha: 1 80 | Autocompute Intensity Bounds: true 81 | Autocompute Value Bounds: 82 | Max Value: 10 83 | Min Value: -10 84 | Value: true 85 | Axis: Z 86 | Channel Name: intensity 87 | Class: rviz/LaserScan 88 | Color: 255; 255; 255 89 | Color Transformer: Intensity 90 | Decay Time: 0 91 | Enabled: true 92 | Invert Rainbow: false 93 | Max Color: 255; 255; 255 94 | Max Intensity: 0 95 | Min Color: 0; 0; 0 96 | Min Intensity: 0 97 | Name: LaserScan 98 | Position Transformer: XYZ 99 | Queue Size: 10 100 | Selectable: true 101 | Size (Pixels): 3 102 | Size (m): 0.05000000074505806 103 | Style: Flat Squares 104 | Topic: /scan 105 | Unreliable: false 106 | Use Fixed Frame: true 107 | Use rainbow: false 108 | Value: true 109 | - Alpha: 1 110 | Axes Length: 1 111 | Axes Radius: 0.10000000149011612 112 | Class: rviz/Pose 113 | Color: 0; 255; 0 114 | Enabled: true 115 | Head Length: 0.30000001192092896 116 | Head Radius: 0.10000000149011612 117 | Name: move_base_goal 118 | Shaft Length: 1 119 | Shaft Radius: 0.05000000074505806 120 | Shape: Arrow 121 | Topic: /move_base_simple/goal 122 | Unreliable: false 123 | Value: true 124 | - Angle Tolerance: 0.10000000149011612 125 | Class: rviz/Odometry 126 | Covariance: 127 | Orientation: 128 | Alpha: 0.5 129 | Color: 255; 255; 127 130 | Color Style: Unique 131 | Frame: Local 132 | Offset: 1 133 | Scale: 1 134 | Value: true 135 | Position: 136 | Alpha: 0.30000001192092896 137 | Color: 204; 51; 204 138 | Scale: 1 139 | Value: true 140 | Value: false 141 | Enabled: true 142 | Keep: 100 143 | Name: Odometry 144 | Position Tolerance: 0.10000000149011612 145 | Shape: 146 | Alpha: 1 147 | Axes Length: 1 148 | Axes Radius: 0.10000000149011612 149 | Color: 255; 25; 0 150 | Head Length: 0.10000000149011612 151 | Head Radius: 0.05000000074505806 152 | Shaft Length: 0.30000001192092896 153 | Shaft Radius: 0.05000000074505806 154 | Value: Arrow 155 | Topic: /downward/vio/odometry 156 | Unreliable: false 157 | Value: true 158 | - Angle Tolerance: 0.10000000149011612 159 | Class: rviz/Odometry 160 | Covariance: 161 | Orientation: 162 | Alpha: 0.5 163 | Color: 255; 255; 127 164 | Color Style: Unique 165 | Frame: Local 166 | Offset: 1 167 | Scale: 1 168 | Value: true 169 | Position: 170 | Alpha: 0.30000001192092896 171 | Color: 204; 51; 204 172 | Scale: 1 173 | Value: true 174 | Value: false 175 | Enabled: true 176 | Keep: 400 177 | Name: Goal 178 | Position Tolerance: 0.10000000149011612 179 | Shape: 180 | Alpha: 1 181 | Axes Length: 1 182 | Axes Radius: 0.10000000149011612 183 | Color: 0; 255; 0 184 | Head Length: 0.10000000149011612 185 | Head Radius: 0.10000000149011612 186 | Shaft Length: 0.10000000149011612 187 | Shaft Radius: 0.10000000149011612 188 | Value: Arrow 189 | Topic: /odometry_goal 190 | Unreliable: false 191 | Value: true 192 | - Angle Tolerance: 0.10000000149011612 193 | Class: rviz/Odometry 194 | Covariance: 195 | Orientation: 196 | Alpha: 0.5 197 | Color: 255; 255; 127 198 | Color Style: Unique 199 | Frame: Local 200 | Offset: 1 201 | Scale: 1 202 | Value: false 203 | Position: 204 | Alpha: 0.30000001192092896 205 | Color: 52; 101; 164 206 | Scale: 0.5 207 | Value: true 208 | Value: false 209 | Enabled: true 210 | Keep: 1 211 | Name: Reference 212 | Position Tolerance: 0.10000000149011612 213 | Shape: 214 | Alpha: 1 215 | Axes Length: 1 216 | Axes Radius: 0.10000000149011612 217 | Color: 114; 159; 207 218 | Head Length: 0.10000000149011612 219 | Head Radius: 0.10000000149011612 220 | Shaft Length: 1 221 | Shaft Radius: 0.05000000074505806 222 | Value: Arrow 223 | Topic: /reference_vis 224 | Unreliable: false 225 | Value: true 226 | Enabled: true 227 | Global Options: 228 | Background Color: 238; 238; 236 229 | Default Light: true 230 | Fixed Frame: map 231 | Frame Rate: 30 232 | Name: root 233 | Tools: 234 | - Class: rviz/Interact 235 | Hide Inactive Objects: true 236 | - Class: rviz/MoveCamera 237 | - Class: rviz/Select 238 | - Class: rviz/FocusCamera 239 | - Class: rviz/Measure 240 | - Class: rviz/SetInitialPose 241 | Theta std deviation: 0.2617993950843811 242 | Topic: /initialpose 243 | X std deviation: 0.5 244 | Y std deviation: 0.5 245 | - Class: rviz/SetGoal 246 | Topic: /move_base_simple/goal 247 | - Class: rviz/PublishPoint 248 | Single click: true 249 | Topic: /clicked_point 250 | Value: true 251 | Views: 252 | Current: 253 | Angle: 0.07000001519918442 254 | Class: rviz/TopDownOrtho 255 | Enable Stereo Rendering: 256 | Stereo Eye Separation: 0.05999999865889549 257 | Stereo Focal Distance: 1 258 | Swap Stereo Eyes: false 259 | Value: false 260 | Invert Z Axis: false 261 | Name: Current View 262 | Near Clip Distance: 0.009999999776482582 263 | Scale: 54.692779541015625 264 | Target Frame: base_link 265 | Value: TopDownOrtho (rviz) 266 | X: 0 267 | Y: 0 268 | Saved: ~ 269 | Window Geometry: 270 | Displays: 271 | collapsed: true 272 | Height: 1023 273 | Hide Left Dock: true 274 | Hide Right Dock: true 275 | QMainWindow State: 000000ff00000000fd0000000400000000000001970000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001370000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000040fc0100000002fb0000000800540069006d00650100000000000003c0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 276 | Selection: 277 | collapsed: false 278 | Time: 279 | collapsed: false 280 | Tool Properties: 281 | collapsed: false 282 | Views: 283 | collapsed: true 284 | Width: 960 285 | X: 960 286 | Y: 27 287 | -------------------------------------------------------------------------------- /rviz/rviz_hw.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | - /Goal1 12 | - /Goal1/Shape1 13 | - /Reference1/Shape1 14 | - /Reference1/Covariance1 15 | - /Reference1/Covariance1/Position1 16 | - /LaserScan1 17 | - /Robot Pose1 18 | Splitter Ratio: 0.49864864349365234 19 | Tree Height: 719 20 | - Class: rviz/Selection 21 | Name: Selection 22 | - Class: rviz/Tool Properties 23 | Expanded: 24 | - /2D Pose Estimate1 25 | - /2D Nav Goal1 26 | - /Publish Point1 27 | Name: Tool Properties 28 | Splitter Ratio: 0.5886790156364441 29 | - Class: rviz/Views 30 | Expanded: 31 | - /Current View1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: LaserScan 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 0.5 47 | Cell Size: 1 48 | Class: rviz/Grid 49 | Color: 160; 160; 164 50 | Enabled: true 51 | Line Style: 52 | Line Width: 0.029999999329447746 53 | Value: Lines 54 | Name: Grid 55 | Normal Cell Count: 0 56 | Offset: 57 | X: 0 58 | Y: 0 59 | Z: 0 60 | Plane: XY 61 | Plane Cell Count: 200 62 | Reference Frame: xmaxx1/odom 63 | Value: true 64 | - Alpha: 1 65 | Axes Length: 1 66 | Axes Radius: 0.10000000149011612 67 | Class: rviz/Pose 68 | Color: 0; 255; 0 69 | Enabled: true 70 | Head Length: 0.30000001192092896 71 | Head Radius: 0.10000000149011612 72 | Name: move_base_goal 73 | Shaft Length: 1 74 | Shaft Radius: 0.05000000074505806 75 | Shape: Arrow 76 | Topic: /xmaxx1/move_base_simple/goal 77 | Unreliable: false 78 | Value: true 79 | - Angle Tolerance: 0.10000000149011612 80 | Class: rviz/Odometry 81 | Covariance: 82 | Orientation: 83 | Alpha: 0.5 84 | Color: 255; 255; 127 85 | Color Style: Unique 86 | Frame: Local 87 | Offset: 1 88 | Scale: 1 89 | Value: true 90 | Position: 91 | Alpha: 0.30000001192092896 92 | Color: 204; 51; 204 93 | Scale: 1 94 | Value: true 95 | Value: false 96 | Enabled: true 97 | Keep: 1000 98 | Name: Odometry 99 | Position Tolerance: 0.10000000149011612 100 | Shape: 101 | Alpha: 0.5 102 | Axes Length: 1 103 | Axes Radius: 0.10000000149011612 104 | Color: 255; 25; 0 105 | Head Length: 0.10000000149011612 106 | Head Radius: 0.05000000074505806 107 | Shaft Length: 0.30000001192092896 108 | Shaft Radius: 0.05000000074505806 109 | Value: Arrow 110 | Topic: /xmaxx1/odom_vis 111 | Unreliable: false 112 | Value: true 113 | - Angle Tolerance: 0.10000000149011612 114 | Class: rviz/Odometry 115 | Covariance: 116 | Orientation: 117 | Alpha: 0.5 118 | Color: 255; 255; 127 119 | Color Style: Unique 120 | Frame: Local 121 | Offset: 1 122 | Scale: 1 123 | Value: true 124 | Position: 125 | Alpha: 0.30000001192092896 126 | Color: 204; 51; 204 127 | Scale: 1 128 | Value: true 129 | Value: false 130 | Enabled: true 131 | Keep: 400 132 | Name: Goal 133 | Position Tolerance: 0.10000000149011612 134 | Shape: 135 | Alpha: 1 136 | Axes Length: 1 137 | Axes Radius: 0.10000000149011612 138 | Color: 0; 255; 0 139 | Head Length: 0.10000000149011612 140 | Head Radius: 0.10000000149011612 141 | Shaft Length: 0.10000000149011612 142 | Shaft Radius: 0.10000000149011612 143 | Value: Arrow 144 | Topic: /xmaxx1/odometry_goal 145 | Unreliable: false 146 | Value: true 147 | - Angle Tolerance: 0.10000000149011612 148 | Class: rviz/Odometry 149 | Covariance: 150 | Orientation: 151 | Alpha: 0.5 152 | Color: 255; 255; 127 153 | Color Style: Unique 154 | Frame: Local 155 | Offset: 1 156 | Scale: 1 157 | Value: false 158 | Position: 159 | Alpha: 0.30000001192092896 160 | Color: 52; 101; 164 161 | Scale: 0.5 162 | Value: true 163 | Value: false 164 | Enabled: true 165 | Keep: 1 166 | Name: Reference 167 | Position Tolerance: 0.10000000149011612 168 | Shape: 169 | Alpha: 1 170 | Axes Length: 1 171 | Axes Radius: 0.10000000149011612 172 | Color: 114; 159; 207 173 | Head Length: 0.10000000149011612 174 | Head Radius: 0.10000000149011612 175 | Shaft Length: 1 176 | Shaft Radius: 0.05000000074505806 177 | Value: Arrow 178 | Topic: /xmaxx1/reference_vis 179 | Unreliable: false 180 | Value: true 181 | - Alpha: 1 182 | Autocompute Intensity Bounds: true 183 | Autocompute Value Bounds: 184 | Max Value: 0.7649313807487488 185 | Min Value: 0.07800853252410889 186 | Value: true 187 | Axis: Z 188 | Channel Name: intensity 189 | Class: rviz/PointCloud2 190 | Color: 255; 255; 255 191 | Color Transformer: AxisColor 192 | Decay Time: 0 193 | Enabled: false 194 | Invert Rainbow: false 195 | Max Color: 255; 255; 255 196 | Max Intensity: 104 197 | Min Color: 0; 0; 0 198 | Min Intensity: 0 199 | Name: Velodyne/Local_obstacles 200 | Position Transformer: XYZ 201 | Queue Size: 10 202 | Selectable: true 203 | Size (Pixels): 3 204 | Size (m): 0.019999999552965164 205 | Style: Flat Squares 206 | Topic: /xmaxx1/velodyne_points/local_obstacles 207 | Unreliable: false 208 | Use Fixed Frame: true 209 | Use rainbow: true 210 | Value: false 211 | - Class: rviz/Image 212 | Enabled: false 213 | Image Topic: /xmaxx1/camera_front/color/image_raw_throttle 214 | Max Value: 1 215 | Median window: 5 216 | Min Value: 0 217 | Name: Image 218 | Normalize Range: true 219 | Queue Size: 2 220 | Transport Hint: raw 221 | Unreliable: false 222 | Value: false 223 | - Alpha: 1 224 | Autocompute Intensity Bounds: true 225 | Autocompute Value Bounds: 226 | Max Value: 10 227 | Min Value: -10 228 | Value: true 229 | Axis: Z 230 | Channel Name: intensity 231 | Class: rviz/LaserScan 232 | Color: 239; 41; 41 233 | Color Transformer: FlatColor 234 | Decay Time: 0 235 | Enabled: true 236 | Invert Rainbow: false 237 | Max Color: 255; 255; 255 238 | Max Intensity: 100 239 | Min Color: 0; 0; 0 240 | Min Intensity: 0 241 | Name: LaserScan 242 | Position Transformer: XYZ 243 | Queue Size: 10 244 | Selectable: true 245 | Size (Pixels): 3 246 | Size (m): 0.10000000149011612 247 | Style: Flat Squares 248 | Topic: /xmaxx1/scan 249 | Unreliable: false 250 | Use Fixed Frame: true 251 | Use rainbow: false 252 | Value: true 253 | - Class: rviz/Axes 254 | Enabled: false 255 | Length: 1 256 | Name: Robot Pose 257 | Radius: 0.10000000149011612 258 | Reference Frame: xmaxx1/base_link 259 | Value: false 260 | Enabled: true 261 | Global Options: 262 | Background Color: 238; 238; 236 263 | Default Light: true 264 | Fixed Frame: xmaxx1/odom 265 | Frame Rate: 30 266 | Name: root 267 | Tools: 268 | - Class: rviz/Interact 269 | Hide Inactive Objects: true 270 | - Class: rviz/MoveCamera 271 | - Class: rviz/Select 272 | - Class: rviz/FocusCamera 273 | - Class: rviz/Measure 274 | - Class: rviz/SetInitialPose 275 | Theta std deviation: 0.2617993950843811 276 | Topic: /initialpose 277 | X std deviation: 0.5 278 | Y std deviation: 0.5 279 | - Class: rviz/SetGoal 280 | Topic: /xmaxx1/move_base_simple/goal 281 | - Class: rviz/PublishPoint 282 | Single click: true 283 | Topic: /clicked_point 284 | Value: true 285 | Views: 286 | Current: 287 | Angle: 0 288 | Class: rviz/TopDownOrtho 289 | Enable Stereo Rendering: 290 | Stereo Eye Separation: 0.05999999865889549 291 | Stereo Focal Distance: 1 292 | Swap Stereo Eyes: false 293 | Value: false 294 | Invert Z Axis: false 295 | Name: Current View 296 | Near Clip Distance: 0.009999999776482582 297 | Scale: 67.67170715332031 298 | Target Frame: xmaxx1/odom 299 | Value: TopDownOrtho (rviz) 300 | X: 7.895087718963623 301 | Y: 0.14680951833724976 302 | Saved: ~ 303 | Window Geometry: 304 | Displays: 305 | collapsed: false 306 | Height: 1025 307 | Hide Left Dock: false 308 | Hide Right Dock: false 309 | Image: 310 | collapsed: false 311 | QMainWindow State: 000000ff00000000fd00000004000000000000028500000361fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002140000018a0000001600ffffff000000010000024a00000361fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000361000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004f50000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 312 | Selection: 313 | collapsed: false 314 | Time: 315 | collapsed: false 316 | Tool Properties: 317 | collapsed: false 318 | Views: 319 | collapsed: false 320 | Width: 1920 321 | X: 0 322 | Y: 27 323 | -------------------------------------------------------------------------------- /rviz/rviz_plotting.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | - /Goal1 12 | - /Goal1/Shape1 13 | - /Reference1 14 | - /Reference1/Shape1 15 | - /Reference1/Covariance1 16 | - /Reference1/Covariance1/Position1 17 | - /Velodyne/Local_obstacles1 18 | - /LaserScan1 19 | - /Odometry2 20 | - /Odometry3 21 | - /Odometry3/Shape1 22 | Splitter Ratio: 0.49864864349365234 23 | Tree Height: 719 24 | - Class: rviz/Selection 25 | Name: Selection 26 | - Class: rviz/Tool Properties 27 | Expanded: 28 | - /2D Pose Estimate1 29 | - /2D Nav Goal1 30 | - /Publish Point1 31 | Name: Tool Properties 32 | Splitter Ratio: 0.5886790156364441 33 | - Class: rviz/Views 34 | Expanded: 35 | - /Current View1 36 | Name: Views 37 | Splitter Ratio: 0.5 38 | - Class: rviz/Time 39 | Experimental: false 40 | Name: Time 41 | SyncMode: 0 42 | SyncSource: "" 43 | Preferences: 44 | PromptSaveOnExit: true 45 | Toolbars: 46 | toolButtonStyle: 2 47 | Visualization Manager: 48 | Class: "" 49 | Displays: 50 | - Alpha: 0.5 51 | Cell Size: 1 52 | Class: rviz/Grid 53 | Color: 160; 160; 164 54 | Enabled: true 55 | Line Style: 56 | Line Width: 0.029999999329447746 57 | Value: Lines 58 | Name: Grid 59 | Normal Cell Count: 0 60 | Offset: 61 | X: 0 62 | Y: 0 63 | Z: 0 64 | Plane: XY 65 | Plane Cell Count: 200 66 | Reference Frame: xmaxx1/odom 67 | Value: true 68 | - Alpha: 1 69 | Axes Length: 1 70 | Axes Radius: 0.10000000149011612 71 | Class: rviz/Pose 72 | Color: 0; 255; 0 73 | Enabled: true 74 | Head Length: 0.30000001192092896 75 | Head Radius: 0.10000000149011612 76 | Name: move_base_goal 77 | Shaft Length: 1 78 | Shaft Radius: 0.05000000074505806 79 | Shape: Arrow 80 | Topic: /xmaxx1/move_base_simple/goal 81 | Unreliable: false 82 | Value: true 83 | - Angle Tolerance: 0.10000000149011612 84 | Class: rviz/Odometry 85 | Covariance: 86 | Orientation: 87 | Alpha: 0.5 88 | Color: 255; 255; 127 89 | Color Style: Unique 90 | Frame: Local 91 | Offset: 1 92 | Scale: 1 93 | Value: true 94 | Position: 95 | Alpha: 0.30000001192092896 96 | Color: 204; 51; 204 97 | Scale: 1 98 | Value: true 99 | Value: false 100 | Enabled: true 101 | Keep: 10000 102 | Name: Odometry 103 | Position Tolerance: 0.10000000149011612 104 | Shape: 105 | Alpha: 0.10000000149011612 106 | Axes Length: 1 107 | Axes Radius: 0.10000000149011612 108 | Color: 255; 25; 0 109 | Head Length: 0.10000000149011612 110 | Head Radius: 0.05000000074505806 111 | Shaft Length: 0.30000001192092896 112 | Shaft Radius: 0.029999999329447746 113 | Value: Arrow 114 | Topic: /xmaxx1/camera/odom/sample 115 | Unreliable: false 116 | Value: true 117 | - Angle Tolerance: 0.10000000149011612 118 | Class: rviz/Odometry 119 | Covariance: 120 | Orientation: 121 | Alpha: 0.5 122 | Color: 255; 255; 127 123 | Color Style: Unique 124 | Frame: Local 125 | Offset: 1 126 | Scale: 1 127 | Value: true 128 | Position: 129 | Alpha: 0.30000001192092896 130 | Color: 204; 51; 204 131 | Scale: 1 132 | Value: true 133 | Value: false 134 | Enabled: true 135 | Keep: 4000 136 | Name: Goal 137 | Position Tolerance: 0.10000000149011612 138 | Shape: 139 | Alpha: 1 140 | Axes Length: 1 141 | Axes Radius: 0.10000000149011612 142 | Color: 0; 12; 255 143 | Head Length: 0.05000000074505806 144 | Head Radius: 0.05000000074505806 145 | Shaft Length: 0.10000000149011612 146 | Shaft Radius: 0.05000000074505806 147 | Value: Arrow 148 | Topic: /xmaxx1/odometry_goal 149 | Unreliable: false 150 | Value: true 151 | - Angle Tolerance: 0.10000000149011612 152 | Class: rviz/Odometry 153 | Covariance: 154 | Orientation: 155 | Alpha: 0.5 156 | Color: 255; 255; 127 157 | Color Style: Unique 158 | Frame: Local 159 | Offset: 1 160 | Scale: 1 161 | Value: false 162 | Position: 163 | Alpha: 0.30000001192092896 164 | Color: 52; 101; 164 165 | Scale: 0.5 166 | Value: true 167 | Value: false 168 | Enabled: false 169 | Keep: 1 170 | Name: Reference 171 | Position Tolerance: 0.10000000149011612 172 | Shape: 173 | Alpha: 1 174 | Axes Length: 1 175 | Axes Radius: 0.10000000149011612 176 | Color: 114; 159; 207 177 | Head Length: 0.10000000149011612 178 | Head Radius: 0.10000000149011612 179 | Shaft Length: 1 180 | Shaft Radius: 0.05000000074505806 181 | Value: Arrow 182 | Topic: /xmaxx1/reference_vis 183 | Unreliable: false 184 | Value: false 185 | - Alpha: 1 186 | Autocompute Intensity Bounds: true 187 | Autocompute Value Bounds: 188 | Max Value: 1.3567140102386475 189 | Min Value: -0.134757399559021 190 | Value: true 191 | Axis: Z 192 | Channel Name: intensity 193 | Class: rviz/PointCloud2 194 | Color: 255; 255; 255 195 | Color Transformer: Intensity 196 | Decay Time: 0 197 | Enabled: false 198 | Invert Rainbow: false 199 | Max Color: 255; 255; 255 200 | Max Intensity: 57 201 | Min Color: 0; 0; 0 202 | Min Intensity: 0 203 | Name: Velodyne/Local_obstacles 204 | Position Transformer: XYZ 205 | Queue Size: 10 206 | Selectable: true 207 | Size (Pixels): 3 208 | Size (m): 0.05000000074505806 209 | Style: Flat Squares 210 | Topic: /xmaxx1/velodyne_points/local_obstacles 211 | Unreliable: false 212 | Use Fixed Frame: true 213 | Use rainbow: true 214 | Value: false 215 | - Class: rviz/Image 216 | Enabled: false 217 | Image Topic: /xmaxx1/camera_front/color/image_raw_throttle 218 | Max Value: 1 219 | Median window: 5 220 | Min Value: 0 221 | Name: Image 222 | Normalize Range: true 223 | Queue Size: 2 224 | Transport Hint: raw 225 | Unreliable: false 226 | Value: false 227 | - Alpha: 1 228 | Autocompute Intensity Bounds: true 229 | Autocompute Value Bounds: 230 | Max Value: 10 231 | Min Value: -10 232 | Value: true 233 | Axis: Z 234 | Channel Name: intensity 235 | Class: rviz/LaserScan 236 | Color: 239; 41; 41 237 | Color Transformer: FlatColor 238 | Decay Time: 0 239 | Enabled: false 240 | Invert Rainbow: false 241 | Max Color: 255; 255; 255 242 | Max Intensity: 100 243 | Min Color: 0; 0; 0 244 | Min Intensity: 0 245 | Name: LaserScan 246 | Position Transformer: XYZ 247 | Queue Size: 10 248 | Selectable: true 249 | Size (Pixels): 3 250 | Size (m): 0.05000000074505806 251 | Style: Flat Squares 252 | Topic: /xmaxx1/scan 253 | Unreliable: false 254 | Use Fixed Frame: true 255 | Use rainbow: false 256 | Value: false 257 | - Angle Tolerance: 0.10000000149011612 258 | Class: rviz/Odometry 259 | Covariance: 260 | Orientation: 261 | Alpha: 0.5 262 | Color: 255; 255; 127 263 | Color Style: Unique 264 | Frame: Local 265 | Offset: 1 266 | Scale: 1 267 | Value: true 268 | Position: 269 | Alpha: 0.30000001192092896 270 | Color: 204; 51; 204 271 | Scale: 1 272 | Value: true 273 | Value: true 274 | Enabled: false 275 | Keep: 100 276 | Name: Odometry 277 | Position Tolerance: 0.10000000149011612 278 | Shape: 279 | Alpha: 1 280 | Axes Length: 1 281 | Axes Radius: 0.10000000149011612 282 | Color: 255; 25; 0 283 | Head Length: 0.30000001192092896 284 | Head Radius: 0.10000000149011612 285 | Shaft Length: 1 286 | Shaft Radius: 0.05000000074505806 287 | Value: Arrow 288 | Topic: /xmaxx1/camera/odom/sample 289 | Unreliable: false 290 | Value: false 291 | - Angle Tolerance: 0.10000000149011612 292 | Class: rviz/Odometry 293 | Covariance: 294 | Orientation: 295 | Alpha: 0.5 296 | Color: 255; 255; 127 297 | Color Style: Unique 298 | Frame: Local 299 | Offset: 1 300 | Scale: 1 301 | Value: true 302 | Position: 303 | Alpha: 0.30000001192092896 304 | Color: 204; 51; 204 305 | Scale: 1 306 | Value: true 307 | Value: false 308 | Enabled: false 309 | Keep: 10000 310 | Name: Odometry 311 | Position Tolerance: 0.10000000149011612 312 | Shape: 313 | Alpha: 1 314 | Axes Length: 1 315 | Axes Radius: 0.10000000149011612 316 | Color: 0; 255; 0 317 | Head Length: 0.10000000149011612 318 | Head Radius: 0.05000000074505806 319 | Shaft Length: 0.30000001192092896 320 | Shaft Radius: 0.029999999329447746 321 | Value: Arrow 322 | Topic: /xmaxx1/camera/odom/sample 323 | Unreliable: false 324 | Value: false 325 | Enabled: true 326 | Global Options: 327 | Background Color: 255; 255; 255 328 | Default Light: true 329 | Fixed Frame: xmaxx1/odom 330 | Frame Rate: 30 331 | Name: root 332 | Tools: 333 | - Class: rviz/Interact 334 | Hide Inactive Objects: true 335 | - Class: rviz/MoveCamera 336 | - Class: rviz/Select 337 | - Class: rviz/FocusCamera 338 | - Class: rviz/Measure 339 | - Class: rviz/SetInitialPose 340 | Theta std deviation: 0.2617993950843811 341 | Topic: /initialpose 342 | X std deviation: 0.5 343 | Y std deviation: 0.5 344 | - Class: rviz/SetGoal 345 | Topic: /xmaxx1/move_base_simple/goal 346 | - Class: rviz/PublishPoint 347 | Single click: true 348 | Topic: /clicked_point 349 | Value: true 350 | Views: 351 | Current: 352 | Angle: 0.004999992437660694 353 | Class: rviz/TopDownOrtho 354 | Enable Stereo Rendering: 355 | Stereo Eye Separation: 0.05999999865889549 356 | Stereo Focal Distance: 1 357 | Swap Stereo Eyes: false 358 | Value: false 359 | Invert Z Axis: false 360 | Name: Current View 361 | Near Clip Distance: 0.009999999776482582 362 | Scale: 100.394775390625 363 | Target Frame: xmaxx1/odom 364 | Value: TopDownOrtho (rviz) 365 | X: 6.0132060050964355 366 | Y: 1.9741082191467285 367 | Saved: ~ 368 | Window Geometry: 369 | Displays: 370 | collapsed: true 371 | Height: 1025 372 | Hide Left Dock: true 373 | Hide Right Dock: false 374 | Image: 375 | collapsed: false 376 | QMainWindow State: 000000ff00000000fd00000004000000000000028500000361fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002140000018a0000001600ffffff000000010000024a0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 377 | Selection: 378 | collapsed: false 379 | Time: 380 | collapsed: false 381 | Tool Properties: 382 | collapsed: false 383 | Views: 384 | collapsed: false 385 | Width: 1920 386 | X: 0 387 | Y: 27 388 | -------------------------------------------------------------------------------- /scripts/barrier_formulas.m: -------------------------------------------------------------------------------- 1 | syms z1 z2 z3 z4 z5 v_lim x_pos y_pos radius gamma_p 2 | 3 | z = [z1 z2 z3 z4]; 4 | %% 5 | h_velocity = sqrt(z3^2+z4^2)*z5 - v_lim; 6 | dh_velocity = simplify(jacobian(h_velocity,z)) 7 | d2h_velocity = simplify(hessian(h_velocity,z)) 8 | 9 | %% 10 | d = sqrt((z1-x_pos)^2 + (z2-y_pos)^2); 11 | h_pos = gamma_p * (d - radius) + (z1-x_pos) / d * z3 + (z2-y_pos) / d * z4; 12 | dh_pos = (jacobian(h_pos,z)) 13 | d2h_pos = (hessian(h_pos,z)) 14 | 15 | 16 | -------------------------------------------------------------------------------- /scripts/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "### Cleaning the previous session ###" 4 | 5 | rosclean purge -y 6 | 7 | # Kill 8 | killall -9 screen 9 | 10 | # Kill gazebo session 11 | killall -9 gzclient 12 | killall -9 gzserver 13 | 14 | # Kill ROS master 15 | killall -9 rosmaster 16 | killall -9 rosout 17 | 18 | killall -9 default_cfg 19 | killall -9 roslaunch 20 | killall -9 master_discovery 21 | killall -9 master_sync 22 | 23 | killall -9 python 24 | killall -9 robot_state_publisher 25 | 26 | screen -wipe 27 | 28 | # Show status 29 | echo "" 30 | echo "### Remaining ROS processes ###" 31 | ps aux | grep ros | grep -v "grep" 32 | 33 | exit 0 34 | 35 | -------------------------------------------------------------------------------- /scripts/set_physics_props.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosservice call /gazebo/set_physics_properties \ 3 | '{time_step: 0.001, 4 | max_update_rate: 1000.0, 5 | gravity:{ 6 | x: 0.0, 7 | y: 0.0, 8 | z: -9.8}, 9 | ode_config:{ 10 | auto_disable_bodies: False, 11 | sor_pgs_precon_iters: 0, 12 | sor_pgs_iters: 50, 13 | sor_pgs_w: 1.3, 14 | sor_pgs_rms_error_tol: 0.0, 15 | contact_surface_layer: 0.001, 16 | contact_max_correcting_vel: 100.0, 17 | cfm: 0.0, 18 | erp: 0.2, 19 | max_contacts: 20}}' 20 | -------------------------------------------------------------------------------- /scripts/startup_base.yaml: -------------------------------------------------------------------------------- 1 | session_name: xmaxx_base 2 | 3 | # before_script: "../scripts/cleanup.sh" 4 | 5 | # Default environment variables; Overwrite from command line 6 | environment: 7 | TEST_NAME: test 8 | ROBOT_NAME: xmaxx1 9 | 10 | options: 11 | default-command: /bin/bash 12 | 13 | windows: 14 | - window_name: xmaxx_base 15 | layout: tiled 16 | panes: 17 | - shell_command: 18 | - sleep 2 19 | - rosservice call /xmaxx1/move_base/clear_costmaps "{}" \ 20 | - shell_command: 21 | - sleep 2 22 | - rosservice call /xmaxx1/voxblox_node/clear_map "{}" \ 23 | - shell_command: 24 | - sleep 2 25 | - rostopic pub -r 10 /xmaxx1/e_stop std_msgs/Bool true \ 26 | - shell_command: 27 | - sleep 2 28 | - source /opt/ros/melodic/setup.bash 29 | - node_manager \ 30 | - shell_command: 31 | - sleep 2 32 | - rosrun rqt_reconfigure rqt_reconfigure 33 | - shell_command: 34 | - sleep 2 35 | - rqt_multiplot --multiplot-config $(rospack find controller_adaptiveclbf)/scripts/rqt_multiplot.xml 36 | - shell_command: 37 | - sleep 2 38 | - rostopic pub -r 10 /xmaxx1/heartbeat_base std_msgs/Empty "{}" 39 | - shell_command: 40 | - sleep 2 41 | - ROS_NAMESPACE=xmaxx1 rosrun joy joy_node 42 | - shell_command: 43 | - sleep 2 44 | - roscd controller_adaptiveclbf 45 | - rviz rviz -d rviz/rviz_hw.rviz 46 | - shell_command: 47 | - sleep 2 48 | - rostopic echo /xmaxx1/ackermann_cmd_openloop 49 | -------------------------------------------------------------------------------- /scripts/startup_hw.yaml: -------------------------------------------------------------------------------- 1 | session_name: xmaxx 2 | 3 | # before_script: "../scripts/cleanup.sh" 4 | 5 | # Default environment variables; Overwrite from command line 6 | environment: 7 | TEST_NAME: test 8 | ROBOT_NAME: xmaxx1 9 | 10 | options: 11 | default-command: /bin/bash 12 | 13 | windows: 14 | - window_name: xmaxx 15 | layout: tiled 16 | shell_command_before: 17 | - export DATA_DIR=/data/ros/${ROBOT_NAME}_$(date "+%Y-%m-%d-%H-%M-00")_${TEST_NAME} 18 | - export ROS_LOG_DIR=$DATA_DIR/log 19 | panes: 20 | - shell_command: 21 | - roscore 22 | - shell_command: 23 | - sleep 5 24 | - roslaunch bringup_ugv xmaxx.launch robot_namespace:=$ROBOT_NAME 25 | - shell_command: 26 | - sleep 2 27 | - rosservice call /xmaxx1/move_base/clear_costmaps "{}"; rosservice call /xmaxx1/voxblox_node/clear_map "{}" \ 28 | - shell_command: 29 | - sleep 3 30 | - roslaunch realsense2_camera rs_camera.launch robot_namespace:=${ROBOT_NAME} serial_no:=828112073566 31 | - shell_command: 32 | - sleep 5 33 | - export ROS_NAMESPACE=$ROBOT_NAME 34 | - rosrun e_stop e_stop & 35 | - rostopic pub -r 10 /xmaxx1/e_stop std_msgs/Bool true \ 36 | #- shell_command: 37 | # - sleep 2 38 | # - source /opt/ros/melodic/setup.bash 39 | # - node_manager \ 40 | - shell_command: 41 | - sleep 2 42 | - rosrun tf static_transform_publisher -0.55 0 -0.1 0 0 0 ${ROBOT_NAME}/camera_pose ${ROBOT_NAME}/base_link 100 43 | - shell_command: 44 | - sleep 3 45 | - roslaunch realsense2_camera rs_t265.launch robot_namespace:=${ROBOT_NAME} serial_no:=909212110360 46 | - shell_command: 47 | - sleep 2 48 | - ROS_NAMESPACE=xmaxx1 roslaunch xmaxx_move_base move_base.launch robot_namespace:=${ROBOT_NAME} \ 49 | - window_name: controller 50 | layout: tiled 51 | panes: 52 | - shell_command: 53 | - sleep 5 54 | - roslaunch controller_adaptiveclbf controller_adaptiveclbf.launch robot_namespace:=${ROBOT_NAME} 55 | - shell_command: 56 | - sleep 5 57 | - roslaunch controller_adaptiveclbf model_service.launch robot_namespace:=${ROBOT_NAME} 58 | - shell_command: 59 | - sleep 5 60 | - roslaunch controller_adaptiveclbf figure_eight.launch robot_namespace:=${ROBOT_NAME} 61 | - shell_command: 62 | - sleep 2 63 | - rosrun rqt_reconfigure rqt_reconfigure \ 64 | - shell_command: 65 | - sleep 2 66 | - rqt_multiplot --multiplot-config $(rospack find controller_adaptiveclbf)/scripts/rqt_multiplot.xml \ 67 | 68 | 69 | #- window_name: TX2 70 | # layout: tiled 71 | # shell_command_before: 72 | #- ssh $ROBOT_NAME-artifact 73 | #panes: 74 | #- shell_command: 75 | # - timeout 5 rosrun tmux_scripts cleanup.sh 76 | # - roslaunch bringup_ugv husky_artifact.launch robot_namespace:=$ROBOT_NAME 77 | #- clear 78 | -------------------------------------------------------------------------------- /scripts/startup_sim.yaml: -------------------------------------------------------------------------------- 1 | session_name: xmaxx_sim 2 | windows: 3 | - window_name: xmaxx_sim 4 | layout: tiled 5 | shell_command_before: 6 | - cd ~/ # run as a first command in all panes 7 | panes: 8 | - shell_command: 9 | - roscore 10 | - rosparam set use_sim_time true 11 | # - shell_command: 12 | # - rosrun controller_adaptiveclbf bag_data.py 13 | - shell_command: 14 | - sleep 5 15 | # - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=racecar_moon gui:=true x:=0 y:=-20 16 | # - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=racecar gui:=true x:=0 y:=0 17 | - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=racecar gui:=true x:=0 y:=0 18 | # - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=track_porto gui:=false x:=0 y:=0 19 | # - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=track_porto_bumpy gui:=true x:=0 y:=0 20 | # - DISABLE_GAZEBO_CAMERA=true DISABLE_GAZEBO_GPS=true roslaunch racecar_gazebo racecar.launch world_name:=track_barca gui:=true x:=0 y:=0 21 | # - roslaunch racecar_gazebo racecar.launch world_name:=racecar_subway 22 | - shell_command: 23 | - sleep 3 24 | - roslaunch controller_adaptiveclbf model_service.launch 25 | - shell_command: 26 | - sleep 3 27 | - roslaunch controller_adaptiveclbf controller_adaptiveclbf.launch 28 | - shell_command: 29 | - sleep 6 30 | - roslaunch controller_adaptiveclbf figure_eight.launch 31 | # - "rostopic pub -r 50 /move_base_simple/goal geometry_msgs/PoseStamped '{header: {frame_id: base_link}, pose: {position: {x: 0.0, y: 0}, orientation: {w: 1.0}}}'" 32 | - shell_command: 33 | - sleep 3 34 | - roslaunch controller_adaptiveclbf encoder_odometry.launch 35 | - shell_command: 36 | - sleep 3 37 | - rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link imu 100 38 | - shell_command: 39 | - sleep 3 40 | # - roslaunch racecar_rviz view_racecar.launch 41 | - roscd controller_adaptiveclbf 42 | - rviz rviz -d rviz/racecar_sinusoid.rviz 43 | # - shell_command: 44 | # - sleep 3 45 | # - rostopic echo /ackermann_cmd 46 | # - shell_command: 47 | # - sleep 3 48 | # - rostopic echo /downward/vio/odometry/pose/pose 49 | # - shell_command: 50 | # - sleep 3 51 | # - rostopic echo /downward/vio/odometry/twist/twist 52 | - shell_command: 53 | - sleep 7 54 | # - rostopic pub -r 10 /ackermann_cmd \ 55 | # - rosrun dynamic_reconfigure dynparam set /adaptive_clbf use_model True \ 56 | - rosrun rqt_reconfigure rqt_reconfigure 57 | - shell_command: 58 | - sleep 3 59 | - rqt_multiplot --multiplot-config $(rospack find controller_adaptiveclbf)/scripts/rqt_multiplot.xml 60 | 61 | - shell_command: 62 | - sleep 10 63 | - rosrun controller_adaptiveclbf set_physics_props.sh 64 | -------------------------------------------------------------------------------- /src/ALPaCA/README.md: -------------------------------------------------------------------------------- 1 | ## ALPaCA 2 | 3 | Code from: https://github.com/StanfordASL/ALPaCA 4 | 5 | For "Meta-Learning Priors for Efficient Online Bayesian Regression" by James Harrison, Apoorva Sharma, and Marco Pavone 6 | 7 | ## TODO 8 | 9 | requirements.txt 10 | -------------------------------------------------------------------------------- /src/ALPaCA/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddfan/balsa/6861adf817ce254d5964b35b935ef2f850889ec0/src/ALPaCA/__init__.py -------------------------------------------------------------------------------- /src/ALPaCA/alpaca.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import numpy as np 3 | import time 4 | from copy import deepcopy 5 | from tensorflow.python.ops.parallel_for import gradients 6 | 7 | 8 | class ALPaCA: 9 | def __init__(self, config, sess, graph=None, preprocess=None, f_nom=None): 10 | self.config = deepcopy(config) 11 | self.lr = config['lr'] 12 | self.x_dim = config['x_dim'] 13 | self.phi_dim = config['nn_layers'][-1] 14 | self.y_dim = config['y_dim'] 15 | self.sigma_eps = self.config['sigma_eps'] 16 | 17 | self.updates_so_far = 0 18 | self.sess = sess 19 | self.graph = graph if graph is not None else tf.get_default_graph() 20 | 21 | # y = K^T phi( preprocess(x) ) + f_nom(x) 22 | self.preprocess = preprocess 23 | self.f_nom = f_nom 24 | 25 | def construct_model(self): 26 | with self.graph.as_default(): 27 | last_layer = self.config['nn_layers'][-1] 28 | 29 | if self.sigma_eps is list: 30 | self.SigEps = tf.diag( np.array(self.sigma_eps) ) 31 | else: 32 | self.SigEps = self.sigma_eps*tf.eye(self.y_dim) 33 | self.SigEps = tf.reshape(self.SigEps, (1,1,self.y_dim,self.y_dim)) 34 | 35 | # try making it learnable 36 | #self.SigEps = tf.get_variable('sigma_eps', initializer=self.SigEps ) 37 | 38 | # Prior Parameters of last layer 39 | self.K = tf.get_variable('K_init',shape=[last_layer,self.y_dim]) #\bar{K}_0 40 | 41 | self.L_asym = tf.get_variable('L_asym',shape=[last_layer,last_layer]) # cholesky decomp of \Lambda_0 42 | self.L = tf.matmul(self.L_asym,tf.transpose(self.L_asym)) # \Lambda_0 43 | 44 | # x: query points (M, N_test, x_dim) 45 | # y: target points (M, N_test, y_dim) ( what K^T phi(x) should approximate ) 46 | self.x = tf.placeholder(tf.float32, shape=[None,None,self.x_dim], name="x") 47 | self.y = tf.placeholder(tf.float32, shape=[None,None,self.y_dim], name="y") 48 | 49 | # Points used to compute posterior using BLR 50 | # context_x: x points available for context (M, N_context, x_dim) 51 | # context_y: y points available for context (M, N_context, y_dim) 52 | self.context_x = tf.placeholder(tf.float32, shape=[None,None,self.x_dim], name="cx") 53 | self.context_y = tf.placeholder(tf.float32, shape=[None,None,self.y_dim], name="cy") 54 | 55 | # num_updates: number of context points from context_x,y to use when computing posterior. size (M,) 56 | self.num_models = tf.shape(self.context_x)[0] 57 | self.max_num_context = tf.shape(self.context_x)[1]*tf.ones((self.num_models,), dtype=tf.int32) 58 | self.num_context = tf.placeholder_with_default(self.max_num_context, shape=(None,)) 59 | 60 | # Map input to feature space 61 | with tf.variable_scope('phi',reuse=None): 62 | # self.phi is (M, N_test, phi_dim) 63 | self.phi = tf.map_fn( lambda x: self.basis(x), 64 | elems=self.x, 65 | dtype=tf.float32) 66 | 67 | # Map context input to feature space 68 | with tf.variable_scope('phi', reuse=True): 69 | # self.context_phi is (M, N_context, phi_dim) 70 | self.context_phi = tf.map_fn( lambda x: self.basis(x), 71 | elems=self.context_x, 72 | dtype=tf.float32) 73 | 74 | # Evaluate f_nom if given, else use 0 75 | self.f_nom_cx = tf.zeros_like(self.context_y) 76 | self.f_nom_x = 0 #tf.zeros_like(self.y) 77 | if self.f_nom is not None: 78 | self.f_nom_cx = self.f_nom(self.context_x) 79 | self.f_nom_x = self.f_nom(self.x) 80 | 81 | # Subtract f_nom from context points before BLR 82 | self.context_y_blr = self.context_y - self.f_nom_cx 83 | 84 | # Compute posterior weights from context data 85 | with tf.variable_scope('blr', reuse=None): 86 | # posterior_K is (M, phi_dim, y_dim), posterior_L_inv is (M, phi_dim, phi_dim) 87 | self.posterior_K, self.posterior_L_inv = tf.map_fn( lambda x: self.batch_blr(*x), 88 | elems=(self.context_phi, self.context_y_blr, self.num_context), 89 | dtype=(tf.float32, tf.float32) ) 90 | 91 | 92 | # Compute posterior predictive distribution, and evaluate targets self.y under this distribution 93 | self.mu_pred, self.Sig_pred, self.predictive_nll = self.compute_pred_and_nll() 94 | 95 | # The loss function is expectation of this predictive nll. 96 | self.total_loss = tf.reduce_mean(self.predictive_nll) 97 | tf.summary.scalar('total_loss', self.total_loss) 98 | 99 | self.optimizer = tf.train.AdamOptimizer(self.lr) 100 | 101 | global_step = tf.Variable(0, trainable=False, name='global_step') 102 | self.train_op = self.optimizer.minimize(self.total_loss,global_step=global_step) 103 | 104 | self.train_writer = tf.summary.FileWriter('summaries/'+str(time.time()), self.sess.graph, flush_secs=10) 105 | self.merged = tf.summary.merge_all() 106 | 107 | self.saver = tf.train.Saver() 108 | 109 | self.sess.run(tf.global_variables_initializer()) 110 | 111 | # ---- TF operations ---- # 112 | def basis(self,x,name='basis'): 113 | layer_sizes = self.config['nn_layers'] 114 | activations = { 115 | 'relu': tf.nn.relu, 116 | 'tanh': tf.nn.tanh, 117 | 'sigmoid': tf.nn.sigmoid 118 | } 119 | activation = activations[ self.config['activation'] ] 120 | 121 | if self.preprocess is None: 122 | inp = x 123 | else: 124 | inp = self.preprocess(x) 125 | with tf.variable_scope(name,reuse=tf.AUTO_REUSE): 126 | for units in layer_sizes: 127 | inp = tf.layers.dense(inputs=inp, units=units,activation=activation) 128 | 129 | return inp 130 | 131 | def batch_blr(self,X,Y,num): 132 | X = X[:num,:] 133 | Y = Y[:num,:] 134 | Ln_inv = tf.matrix_inverse(tf.matmul(tf.transpose(X), X) + self.L) 135 | Kn = tf.matmul(Ln_inv, (tf.matmul(tf.transpose(X), Y) + tf.matmul(self.L, self.K))) 136 | return tf.cond( num > 0, lambda : (Kn,Ln_inv), lambda : (self.K, tf.linalg.inv(self.L)) ) 137 | 138 | def compute_pred_and_nll(self): 139 | """ 140 | Uses self.posterior_K and self.posterior_L_inv and self.f_nom_x to generate the posterior predictive. 141 | Returns: 142 | mu_pred = posterior predictive mean at query points self.x 143 | shape (M, T, y_dim) 144 | Sig_pred = posterior predictive variance at query points self.x 145 | shape (M, T, y_dim, y_dim) 146 | predictive_nll = negative log likelihood of self.y under the posterior predictive density 147 | shape (M, T) 148 | """ 149 | mu_pred = batch_matmul(tf.matrix_transpose(self.posterior_K), self.phi) + self.f_nom_x 150 | spread_fac = 1 + batch_quadform(self.posterior_L_inv, self.phi) 151 | Sig_pred = tf.expand_dims(spread_fac, axis=-1)*tf.reshape(self.SigEps, (1,1,self.y_dim,self.y_dim)) 152 | 153 | # Score self.y under predictive distribution to obtain loss 154 | with tf.variable_scope('loss', reuse=None): 155 | logdet = self.y_dim*tf.log(spread_fac) + tf.linalg.logdet(self.SigEps) 156 | Sig_pred_inv = tf.linalg.inv(Sig_pred) 157 | quadf = batch_quadform(Sig_pred_inv, (self.y - mu_pred)) 158 | 159 | predictive_nll = tf.squeeze(logdet + quadf, axis=-1) 160 | 161 | # log stuff for summaries 162 | self.rmse_1 = tf.reduce_mean( tf.sqrt( tf.reduce_sum( tf.square(mu_pred - self.y)[:,0,:], axis=-1 ) ) ) 163 | self.mpv_1 = tf.reduce_mean( tf.matrix_determinant( Sig_pred[:,0,:,:]) ) 164 | tf.summary.scalar('RMSE_1step', self.rmse_1) 165 | tf.summary.scalar('MPV_1step', self.mpv_1) 166 | 167 | return mu_pred, Sig_pred, predictive_nll 168 | 169 | 170 | # ---- Train and Test functions ------ # 171 | def train(self, dataset, num_train_updates, config): 172 | self.config = config 173 | 174 | batch_size = self.config['meta_batch_size'] 175 | horizon = self.config['data_horizon'] 176 | test_horizon = self.config['test_horizon'] 177 | 178 | #minimize loss 179 | for i in range(num_train_updates): 180 | x,y = dataset.sample(n_funcs=batch_size, n_samples=horizon+test_horizon) 181 | 182 | feed_dict = { 183 | self.context_y: y[:,:horizon,:], 184 | self.context_x: x[:,:horizon,:], 185 | self.y: y[:,horizon:,:], 186 | self.x: x[:,horizon:,:], 187 | self.num_context: np.random.randint(horizon+1,size=batch_size) 188 | } 189 | 190 | summary,loss, _ = self.sess.run([self.merged,self.total_loss,self.train_op],feed_dict) 191 | 192 | if i % 50 == 0: 193 | print('loss:',loss) 194 | 195 | self.train_writer.add_summary(summary, self.updates_so_far) 196 | self.updates_so_far += 1 197 | 198 | # x_c, y_c, x are all [N, n] 199 | # returns mu_pred, Sig_pred 200 | def test(self, x_c, y_c, x): 201 | feed_dict = { 202 | self.context_y: y_c, 203 | self.context_x: x_c, 204 | self.x: x 205 | } 206 | mu_pred, Sig_pred = self.sess.run([self.mu_pred, self.Sig_pred], feed_dict) 207 | return mu_pred, Sig_pred 208 | 209 | # convenience function to use just the encoder on numpy input 210 | def encode(self, x): 211 | feed_dict = { 212 | self.x: x 213 | } 214 | return self.sess.run(self.phi, feed_dict) 215 | 216 | # ---- Save and Restore ------ 217 | def save(self, model_path): 218 | save_path = self.saver.save(self.sess, model_path) 219 | print('Saved to:', save_path) 220 | 221 | def restore(self, model_path): 222 | self.saver.restore(self.sess, model_path) 223 | print('Restored model from:', model_path) 224 | 225 | 226 | 227 | # given mat [a,b,c,...,N,N] and batch_v [a,b,c,...,M,N], returns [a,b,c,...,M,N] 228 | def batch_matmul(mat, batch_v, name='batch_matmul'): 229 | with tf.name_scope(name): 230 | return tf.matrix_transpose(tf.matmul(mat,tf.matrix_transpose(batch_v))) 231 | 232 | # works for A = [...,n,n] or [...,N,n,n] 233 | # (uses the same matrix A for all N b vectors in the first case) 234 | # assumes b = [...,N,n] 235 | # returns [...,N,1] 236 | def batch_quadform(A, b): 237 | A_dims = A.get_shape().ndims 238 | b_dims = b.get_shape().ndims 239 | b_vec = tf.expand_dims(b, axis=-1) 240 | if A_dims == b_dims + 1: 241 | return tf.squeeze( tf.matmul(tf.matmul(tf.matrix_transpose(b_vec) , A) , b_vec), axis=-1) 242 | elif A_dims == b_dims: 243 | Ab = tf.expand_dims( tf.matrix_transpose( tf.matmul(A , tf.matrix_transpose(b)) ), axis=-1) # ... x N x n x 1 244 | return tf.squeeze( tf.matmul(tf.matrix_transpose(b_vec) , Ab), axis = -1) # ... x N x 1 245 | else: 246 | raise ValueError('Matrix size of %d is not supported.'%(A_dims)) 247 | 248 | # takes in y = (..., y_dim) 249 | # x = (..., x_dim) 250 | # returns dydx = (..., y_dim, x_dim), the jacobian of y wrt x 251 | def batch_2d_jacobian(y, x): 252 | shape = tf.shape(y) 253 | y_dim = y.get_shape().as_list()[-1] 254 | x_dim = x.get_shape().as_list()[-1] 255 | batched_y = tf.reshape(y, (-1, y_dim)) 256 | batched_x = tf.reshape(x, (-1, x_dim)) 257 | 258 | batched_dydx = gradients.batch_jacobian(y, x) 259 | 260 | dydx = tf.reshape(batched_dydx, tf.concat( (shape, [x_dim]), axis=0 )) 261 | return dydx 262 | 263 | #------------ END General ALPaCA -------------# 264 | 265 | def blr_update_np(K,L,X,Y): 266 | Ln_inv = np.linalg.inv( tf.matmul(X.T, X) + L ) 267 | Kn = tf.matmul(Ln_inv , (tf.matmul(X.T , Y) + tf.matmul(L , K))) 268 | return Kn, Ln_inv 269 | 270 | def sampleMN(K, L_inv, Sig): 271 | mean = np.reshape(K.T, [-1]) 272 | cov = np.kron(Sig, L_inv) 273 | K_vec = np.random.multivariate_normal(mean,cov) 274 | return np.reshape(K_vec, K.T.shape).T 275 | 276 | def tp(x): 277 | return np.swapaxes(x, -1,-2) 278 | 279 | def extract_x(xu, x_dim): 280 | xu_shape = tf.shape(xu) 281 | begin = tf.zeros_like(xu_shape) 282 | size = tf.concat( [ -1*tf.ones_like(xu_shape, dtype=tf.int32)[:-1], [x_dim] ], axis=0) 283 | x = tf.slice(xu, begin, size) 284 | return x 285 | 286 | 287 | class AdaptiveDynamics(ALPaCA, object): 288 | def __init__(self, config, sess, graph=None, uncertainty_prop=False, preprocess=None, f_nom=None): 289 | if f_nom == None: 290 | f_nom = lambda xu: extract_x(xu, config['y_dim']) 291 | 292 | super(AdaptiveDynamics, self).__init__(config, sess, graph, preprocess, f_nom) 293 | self.uncertainty_prop = uncertainty_prop 294 | 295 | def construct_model(self, uncertainty_prop=None): 296 | super(AdaptiveDynamics, self).construct_model() 297 | with self.graph.as_default(): 298 | self.dphi_dxu = batch_2d_jacobian(self.phi, self.x) 299 | self.df_nom_dxu = batch_2d_jacobian(self.f_nom_x, self.x) 300 | 301 | def compute_pred_and_nll(self): 302 | # Set up graph to do uncertainty prop / multistep predicitions if requested 303 | if self.uncertainty_prop: 304 | x_dim = self.y_dim 305 | u = self.x[:,:,x_dim:] 306 | initial_sig = tf.zeros((tf.shape(self.x)[0],x_dim,x_dim)) 307 | 308 | mu_pred, Sig_pred = self.rollout_scan(self.x[:,0,:x_dim], initial_sig, u, resampling=True) 309 | 310 | # loss computation 311 | with tf.variable_scope('loss', reuse=None): 312 | logdet = tf.linalg.logdet(Sig_pred) 313 | Sig_pred_inv = tf.linalg.inv(Sig_pred) 314 | quadf = batch_quadform(Sig_pred_inv, (self.y - mu_pred)) 315 | 316 | predictive_nll = logdet + quadf[:,:,0] 317 | 318 | # log stuff for summaries 319 | for t in range(self.config['test_horizon']): 320 | if t % 2 == 0: 321 | rmse = tf.reduce_mean( tf.sqrt( tf.reduce_sum( tf.square(mu_pred - self.y)[:,t,:], axis=-1 ) ) ) 322 | mpv = tf.reduce_mean( tf.matrix_determinant( Sig_pred[:,t,:,:]) ) 323 | tf.summary.scalar('RMSE_'+str(t+1)+'_step', rmse) 324 | tf.summary.scalar('MPV_'+str(t+1)+'_step', mpv) 325 | 326 | return mu_pred, Sig_pred, predictive_nll 327 | 328 | else: 329 | return super(AdaptiveDynamics, self).compute_pred_and_nll() 330 | 331 | # ---- TF utilities for prediction over multiple timesteps with uncertainty propagation ------ 332 | def linearized_step(self, x, S, u, proc_noise=True,resampling=True): 333 | K = self.posterior_K 334 | Linv = self.posterior_L_inv 335 | 336 | xu = tf.concat([x,u],axis=-1) # (M, xdim+u_dim) 337 | 338 | f_nom = tf.zeros_like(x) 339 | if self.f_nom is not None: 340 | f_nom = self.f_nom(xu) 341 | 342 | with tf.variable_scope('phi', reuse=True): 343 | phi = self.basis(xu) # (M, phi_dim) 344 | phi_ed = tf.expand_dims(phi,axis=1) # (M, 1, phi_dim) 345 | 346 | xp = f_nom + batch_matmul( tf.matrix_transpose(K), phi_ed)[:,0,:] 347 | 348 | dxp_dx = tf.stop_gradient( batch_2d_jacobian(xp, x) ) 349 | 350 | # do we need f_nom_x? or can we work with dxp_dx only? 351 | f_nom_x = tf.stop_gradient( batch_2d_jacobian(f_nom, x) ) 352 | phi_x = tf.stop_gradient( batch_2d_jacobian(phi,x) ) 353 | 354 | pxK = tf.matmul(tf.matrix_transpose(phi_x) , K) 355 | pxK_fnomx = pxK + tf.matrix_transpose(f_nom_x) 356 | 357 | Sp = tf.matmul(tf.matmul(dxp_dx , S) , tf.matrix_transpose(dxp_dx)) #tf.matrix_transpose(pxK_fnomx) @ S @ pxK_fnomx 358 | 359 | 360 | if proc_noise: 361 | Sp += self.SigEps[0,:,:,:] # check size on this 362 | 363 | if resampling: 364 | # change shape of trace term so multiply works 365 | trace_term = tf.trace(tf.matmul(tf.matmul(tf.matmul(tf.matrix_transpose(phi_x) , Linv) , phi_x) , S)) 366 | trace_term = tf.reshape(trace_term,[-1,1,1]) 367 | Sp += trace_term * self.SigEps[0,:,:,:] 368 | 369 | # same for quadform term 370 | quadf_term = batch_quadform(Linv, phi_ed) 371 | Sp += quadf_term * self.SigEps[0,:,:,:] 372 | 373 | return xp, Sp 374 | 375 | def rollout_scan(self, x0, S0, u, resampling=True): 376 | u_tp = tf.transpose(u,perm=[1,0,2]) 377 | 378 | init_h = (x0,S0) 379 | 380 | def state_update(ht,ut): 381 | x,S = ht 382 | return self.linearized_step(x,S,ut,resampling=resampling) 383 | 384 | xs, Ss = tf.scan(state_update,u_tp,initializer=init_h) 385 | 386 | xs = tf.transpose(xs,perm=[1,0,2]) 387 | Ss = tf.transpose(Ss,perm=[1,0,2,3]) 388 | 389 | return (xs,Ss) 390 | 391 | # ---- Overloading ALPaCA methods to add gradients and extract useful quanitites from the TF graph ------ 392 | def encode(self, x, return_grad=False): 393 | if not return_grad: 394 | return self.sess.run( self.phi , {self.x: x}) 395 | else: 396 | return self.sess.run( (self.phi, self.dphi_dxu) , {self.x: x}) 397 | 398 | def train(self,dataset,num_train_updates): 399 | super(AdaptiveDynamics, self).train(dataset,num_train_updates) 400 | 401 | # obtain parameters of belief over K as np arrays 402 | self.K0,self.L0,self.sigeps = self.sess.run( (self.K, self.L, self.SigEps[0,0,:,:]) ) 403 | self.reset_to_prior() 404 | 405 | def restore(self, model_path): 406 | super(AdaptiveDynamics, self).restore(model_path) 407 | 408 | # obtain parametrs of belief over K as np arrays 409 | self.K0,self.L0,self.sigeps = self.sess.run( (self.K, self.L, self.SigEps[0,0,:,:]) ) 410 | self.reset_to_prior() 411 | 412 | # ---- Utilities for downstream use of the dynamics model ------- 413 | def gen_predict_func(self): 414 | K = tf.matmul(self.Ln_inv , self.Qn) 415 | 416 | def predict(x,u): 417 | x_inp = np.reshape( np.concatenate((x,u)), (1,1,-1) ) 418 | phi = self.encode(x_inp, return_grad=False) 419 | phi = phi[0,0,:] 420 | f_nom = self.sess.run( self.f_nom_x, {self.x: x_inp} ) 421 | f_nom = f_nom[0,0,:] 422 | return f_nom + tf.matmul(K.T , phi) 423 | 424 | return predict 425 | 426 | # TODO: include f_nom rather than assuming f_nom = x 427 | def gen_linearize_func(self): 428 | K = tf.matmul(self.Ln_inv , self.Qn) 429 | 430 | def linearize(x,u): 431 | batch_size = x.shape[0] 432 | x_dim = x.shape[-1] 433 | u_dim = u.shape[-1] 434 | x_inp = np.reshape( np.concatenate((x,u), axis=-1), (-1,1,x_dim+u_dim) ) 435 | phi, dphi_dxu = self.encode(x_inp, return_grad=True) 436 | phi = phi[:,0,:] 437 | Ap = dphi_dxu[:,0,:,:x_dim] 438 | Bp = dphi_dxu[:,0,:,x_dim:] 439 | 440 | S = np.zeros((batch_size, 1+x_dim+u_dim, 1+x_dim+u_dim)) 441 | Sig = np.zeros((batch_size, x_dim, x_dim)) 442 | A = np.zeros((batch_size, x_dim, x_dim)) 443 | B = np.zeros((batch_size, x_dim, u_dim)) 444 | 445 | for i in range(batch_size): 446 | s = (1 + tf.matmul(tf.matmul(phi[i,:].T , self.Ln_inv) , phi[i,:])) 447 | Sx = tf.matmul(tf.matmul(Ap[i,:,:].T , self.Ln_inv) , phi[i,:]) 448 | Su = tf.matmul(tf.matmul(Bp[i,:,:].T , self.Ln_inv) , phi[i,:]) 449 | Sxx = tf.matmul(tf.matmul(Ap[i,:,:].T , self.Ln_inv) , Ap[i,:,:]) 450 | Suu = tf.matmul(tf.matmul(Bp[i,:,:].T , self.Ln_inv) , Bp[i,:,:]) 451 | Sux = tf.matmul(tf.matmul(Bp[i,:,:].T , self.Ln_inv) , Ap[i,:,:]) 452 | 453 | S[i,0,0] = s 454 | S[i,1:1+x_dim,0] = Sx 455 | S[i,0,1:1+x_dim] = Sx.T 456 | S[i,1+x_dim:,0] = Su 457 | S[i,0,1+x_dim:] = Su.T 458 | S[i,1:1+x_dim,1:1+x_dim] = Sxx 459 | S[i,1:1+x_dim,1+x_dim:] = Sux.T 460 | S[i,1+x_dim:,1:1+x_dim] = Sux 461 | S[i,1+x_dim:,1+x_dim:] = Suu 462 | 463 | A[i,:,:] = tf.matmul(K.T , Ap[i,:,:]) 464 | B[i,:,:] = tf.matmul(K.T , Bp[i,:,:]) 465 | 466 | 467 | Sig[:,:,:] = np.reshape(self.sigeps, (1,x_dim,x_dim)) 468 | 469 | return Sig, S, A + np.eye(x_dim), B 470 | 471 | return linearize 472 | 473 | # TODO: include f_nom rather than assuming f_nom = x (or remove gen_dynamics if not used) 474 | def gen_dynamics(self,sample_model=False): 475 | K = tf.Session().run(tf.matmul(self.Ln_inv , self.Qn)) 476 | 477 | if sample_model: 478 | K = sampleMN(K, self.Ln_inv, self.sigeps) 479 | 480 | def dynamics(x,u): 481 | x_dim = x.shape[-1] 482 | u_dim = u.shape[-1] 483 | x_inp = np.reshape( np.concatenate((x,u)), (1,1,-1) ) 484 | phi, dphi_dxu = self.encode(x_inp, return_grad=True) 485 | phi = phi[0,0,:] 486 | # A = dphi_dxu[0,0,:,:x_dim] 487 | # B = dphi_dxu[0,0,:,x_dim:] 488 | # S = np.zeros((1+x_dim+u_dim, 1+x_dim+u_dim)) 489 | # if sample_model: 490 | # S[0,0] = 1. 491 | # else: 492 | # s = (1 + np.matmul(np.matmul(phi.T , self.Ln_inv) , phi)) 493 | # Sx = tf.matmul(tf.matmul(A.T , self.Ln_inv) , phi) 494 | # Su = tf.matmul(tf.matmul(B.T , self.Ln_inv) , phi) 495 | # Sxx = tf.matmul(tf.matmul(A.T , self.Ln_inv) , A) 496 | # Suu = tf.matmul(tf.matmul(B.T , self.Ln_inv) , B) 497 | # Sux = tf.matmul(tf.matmul(B.T , self.Ln_inv) , A) 498 | # 499 | # S[0,0] = s 500 | # S[1:1+x_dim,0] = Sx 501 | # S[0,1:1+x_dim] = Sx.T 502 | # S[1+x_dim:,0] = Su 503 | # S[0,1+x_dim:] = Su.T 504 | # S[1:1+x_dim,1:1+x_dim] = Sxx 505 | # S[1:1+x_dim,1+x_dim:] = Sux.T 506 | # S[1+x_dim:,1:1+x_dim] = Sux 507 | # S[1+x_dim:,1+x_dim:] = Suu 508 | 509 | 510 | # Sig = s*self.sigeps # original alpaca code 511 | s = (1 + np.matmul(np.matmul(phi.T , self.Ln_inv) , phi)) 512 | Sig = s*self.sigeps 513 | xp = x + np.matmul(K.T, phi) 514 | 515 | # return xp, Sig, S, np.matmul(K.T , A) + np.eye(x_dim), tf.matmul(K.T , B) 516 | return xp, Sig 517 | 518 | return dynamics 519 | 520 | # returns the error of a transition prediction under each model K 521 | def error(self, x, u, xp, Ks): 522 | x_inp = np.reshape( np.concatenate((x,u)), (1,1,-1) ) 523 | phi = self.encode(x_inp, return_grad=False) 524 | phi = phi.reshape((1,-1,1)) 525 | xp_preds = np.squeeze( tf.matmul(tp(Ks) , phi), axis=-1 ) 526 | mses = np.sum( (xp_preds - xp.reshape((1,-1)))**2, axis = -1) 527 | return mses 528 | 529 | # returns a list of N = num_models model parameter samples K1 .. KN 530 | def sample_dynamics_matrices(self, n_models=10): 531 | return np.array( [sampleMN(tf.matmul(self.Ln_inv , self.Qn), self.Ln_inv, self.sigeps) for _ in range(n_models)] ) 532 | 533 | 534 | # applies actions starting at x0, simulating the mean dynamics under each 535 | # model parameter setting K in Ks 536 | # returns: 537 | # x_pred, [N_samples, T+1, x_dim], mean trajectory under each mdoel 538 | # A, [N_samples, T, x_dim, x_dim], (dx'/dx) evaluated at (x_pred[t], 539 | # action[t]) for every t but the last and every model 540 | # B, [N_samples, T, x_dim, u_dim], (dx'/du) 541 | def sample_rollout(self, x0, actions, Ks, resample=False): 542 | T, u_dim = actions.shape 543 | x_dim = x0.shape[0] 544 | N_samples = len(Ks) 545 | 546 | actions = np.tile(np.expand_dims(actions,axis=0), (N_samples, 1, 1)) 547 | x0 = np.expand_dims(x0, axis=0) 548 | 549 | x_pred = np.zeros( (N_samples, T+1, x_dim) ) 550 | x_pred[:,0,:] = x0 551 | 552 | A = np.zeros( (N_samples, T, x_dim, x_dim) ) 553 | B = np.zeros( (N_samples, T, x_dim, u_dim) ) 554 | 555 | for t in range(0, T): 556 | x_inp = np.concatenate( (x_pred[:,t:t+1,:], actions[:,t:t+1,:]), axis=2 ) 557 | phi, dphi_dxu = self.encode(x_inp, return_grad=True) 558 | for j in range(N_samples): 559 | K = Ks[j] 560 | if resample: 561 | K = sampleMN(tf.matmul(self.Ln_inv , self.Qn), self.Ln_inv, self.sigeps) 562 | A[j,t,:,:] = tf.matmul(K.T , dphi_dxu[j,0,:,:x_dim]) 563 | B[j,t,:,:] = tf.matmul(K.T , dphi_dxu[j,0,:,x_dim:]) 564 | x_pred[j,t+1,:] = x_pred[j,t,:] + np.squeeze( tf.matmul(phi[j,:,:] , K) ) 565 | 566 | return x_pred #, A, B 567 | 568 | def reset_to_prior(self): 569 | self.Ln_inv = np.linalg.inv(self.L0) 570 | self.Qn = tf.matmul(self.L0 , self.K0) 571 | 572 | # TODO: need to use f_nom here, not assume f_nom = x 573 | def incorporate_transition(self,x,u,xp): 574 | # perform RLS update to Kn, Ln 575 | x_inp = np.reshape( np.concatenate( (x,u), axis=0 ), (1,1,-1) ) 576 | phi = self.encode(x_inp)[0,:,:].T 577 | 578 | # y = np.reshape(xp - x, (-1,1)) # original alpaca 579 | f_nom = self.sess.run( self.f_nom_x, {self.x: x_inp} ) 580 | f_nom = f_nom[0,0,:] 581 | y = np.reshape(xp - f_nom, (-1,1)) 582 | 583 | phiLamphi = (np.matmul(np.matmul(phi.T , self.Ln_inv) , phi))[0,0] 584 | LninvPhi = np.matmul(self.Ln_inv , phi) 585 | self.Ln_inv = self.Ln_inv - 1./(1. + phiLamphi) * np.matmul(LninvPhi , LninvPhi.T) 586 | self.Qn = np.matmul(phi , y.T) + self.Qn 587 | 588 | # TODO: change to assume Y is yhat + f_nom 589 | def incorporate_batch(self, X, Y): 590 | Phi = self.encode(X) 591 | Kn,Ln_inv = blr_update_np(self.K0,self.L0,Phi[0,:,:],Y[0,:,:]) 592 | self.Ln_inv = Ln_inv 593 | self.Kn = Kn 594 | self.Qn = tf.matmul(np.linalg.inv(self.Ln_inv) , self.Kn) 595 | -------------------------------------------------------------------------------- /src/ALPaCA/dataViz.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import rc 3 | ## for Palatino and other serif fonts use: 4 | rc('font',**{'family':'serif','serif':['Computer Modern Roman']}) 5 | rc('text', usetex=True) 6 | import matplotlib.pyplot as plt 7 | import time 8 | 9 | 10 | #NLL plotting 11 | def nll_plot(nll_mean_list1,nll_var_list1,nll_mean_list2,nll_var_list2,nll_mean_list3,nll_var_list3,N_test,legend=False,last_legend_label=r'GPR'): 12 | 13 | legend_label = [] 14 | if nll_mean_list1 is not None: 15 | plt.gca().set_prop_cycle(None) 16 | 17 | conf_list1 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list1] 18 | upper1 = [y + c for y,c in zip(nll_mean_list1,conf_list1)] 19 | lower1 = [y - c for y,c in zip(nll_mean_list1,conf_list1)] 20 | plt.fill_between(range(0,len(nll_mean_list1)), upper1, lower1, alpha=.2) 21 | plt.plot(range(0,len(nll_mean_list1)),nll_mean_list1) 22 | legend_label.append(r'ALPaCA') 23 | plt.ylabel('Negative Log Likelihood') 24 | 25 | if nll_mean_list2 is not None: 26 | conf_list2 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list2] 27 | upper2 = [y + c for y,c in zip(nll_mean_list2,conf_list2)] 28 | lower2 = [y - c for y,c in zip(nll_mean_list2,conf_list2)] 29 | plt.fill_between(range(0,len(nll_mean_list2)), upper2, lower2, alpha=.2) 30 | plt.plot(range(0,len(nll_mean_list2)),nll_mean_list2) 31 | legend_label.append(r'ALPaCA (no meta)') 32 | 33 | if nll_mean_list3 is not None: 34 | conf_list3 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list3] 35 | upper3 = [y + c for y,c in zip(nll_mean_list3,conf_list3)] 36 | lower3 = [y - c for y,c in zip(nll_mean_list3,conf_list3)] 37 | plt.fill_between(range(0,len(nll_mean_list3)), upper3, lower3, alpha=.2) 38 | plt.plot(range(0,len(nll_mean_list3)),nll_mean_list3) 39 | legend_label.append(last_legend_label) 40 | 41 | if legend==True: 42 | plt.legend(legend_label) 43 | 44 | plt.xlabel('Timesteps') 45 | 46 | def mse_plot(nll_mean_list1,nll_var_list1,nll_mean_list2,nll_var_list2,nll_mean_list3,nll_var_list3,N_test,legend=False): 47 | 48 | legend_label = [] 49 | if nll_mean_list1 is not None: 50 | plt.gca().set_prop_cycle(None) 51 | 52 | conf_list1 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list1] 53 | upper1 = [y + c for y,c in zip(nll_mean_list1,conf_list1)] 54 | lower1 = [y - c for y,c in zip(nll_mean_list1,conf_list1)] 55 | plt.fill_between(range(0,len(nll_mean_list1)), upper1, lower1, alpha=.2) 56 | l1 = plt.plot(range(0,len(nll_mean_list1)),nll_mean_list1,label=r'ALPaCA') 57 | legend_label.append(r'ALPaCA') 58 | plt.ylabel('MSE') 59 | 60 | if nll_mean_list2 is not None: 61 | conf_list2 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list2] 62 | upper2 = [y + c for y,c in zip(nll_mean_list2,conf_list2)] 63 | lower2 = [y - c for y,c in zip(nll_mean_list2,conf_list2)] 64 | plt.fill_between(range(0,len(nll_mean_list2)), upper2, lower2, alpha=.2) 65 | l2 = plt.plot(range(0,len(nll_mean_list2)),nll_mean_list2, label=r'MAML (1 step)') 66 | legend_label.append(r'MAML (1 step)') 67 | 68 | if nll_mean_list3 is not None: 69 | conf_list3 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list3] 70 | upper3 = [y + c for y,c in zip(nll_mean_list3,conf_list3)] 71 | lower3 = [y - c for y,c in zip(nll_mean_list3,conf_list3)] 72 | plt.fill_between(range(0,len(nll_mean_list3)), upper3, lower3, alpha=.2) 73 | plt.plot(range(0,len(nll_mean_list3)),nll_mean_list3, label=r'MAML (5 step)') 74 | legend_label.append(r'GPR') 75 | 76 | if legend==True: 77 | plt.legend() 78 | 79 | plt.xlabel('Timesteps') 80 | 81 | 82 | def time_plot(nll_mean_list1,nll_var_list1,nll_mean_list2,nll_var_list2,nll_mean_list3,nll_var_list3,N_test,legend=False): 83 | #same arguments cause I'm lazy 84 | 85 | legend_label = [] 86 | if nll_mean_list1 is not None: 87 | plt.gca().set_prop_cycle(None) 88 | 89 | conf_list1 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list1] 90 | upper1 = [y + c for y,c in zip(nll_mean_list1,conf_list1)] 91 | lower1 = [y - c for y,c in zip(nll_mean_list1,conf_list1)] 92 | plt.fill_between(range(0,len(nll_mean_list1)), upper1, lower1, alpha=.2) 93 | plt.plot(range(0,len(nll_mean_list1)),nll_mean_list1) 94 | legend_label.append(r'ALPaCA') 95 | plt.ylabel(r'Time (s)') 96 | 97 | if nll_mean_list2 is not None: 98 | conf_list2 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list2] 99 | upper2 = [y + c for y,c in zip(nll_mean_list2,conf_list2)] 100 | lower2 = [y - c for y,c in zip(nll_mean_list2,conf_list2)] 101 | plt.fill_between(range(0,len(nll_mean_list2)), upper2, lower2, alpha=.2) 102 | plt.plot(range(0,len(nll_mean_list2)),nll_mean_list2) 103 | legend_label.append(r'ALPaCA (no meta)') 104 | 105 | if nll_mean_list3 is not None: 106 | conf_list3 = [1.96*np.sqrt(s)/np.sqrt(N_test) for s in nll_var_list3] 107 | upper3 = [y + c for y,c in zip(nll_mean_list3,conf_list3)] 108 | lower3 = [y - c for y,c in zip(nll_mean_list3,conf_list3)] 109 | plt.fill_between(range(0,len(nll_mean_list3)), upper3, lower3, alpha=.2) 110 | plt.plot(range(0,len(nll_mean_list3)),nll_mean_list3) 111 | legend_label.append(r'GPR') 112 | 113 | if legend==True: 114 | plt.legend(legend_label) 115 | 116 | plt.xlabel('Timesteps') 117 | 118 | 119 | def sinusoid_plot(freq,phase,amp,x_list,sigma_list,y_list,X_update, Y_update,sampling_density=101,legend_labels=['Ours', 'True']): 120 | """ 121 | x,y,sigma should be lists 122 | """ 123 | 124 | #plot given data 125 | conf_list = [1.96*np.sqrt(s) for s in sigma_list] 126 | upper = [y + c for y,c in zip(y_list,conf_list)] 127 | lower = [y - c for y,c in zip(y_list,conf_list)] 128 | plt.fill_between(x_list, upper, lower, alpha=.5) 129 | plt.plot(x_list,y_list) 130 | 131 | 132 | 133 | #plot true sinusoid 134 | yr_list = [amp*np.sin(freq*x + phase) for x in x_list] 135 | plt.plot(x_list,yr_list,color='r') 136 | 137 | # plot update points 138 | plt.plot(X_update[0,:,0],Y_update[0,:,0],'+',color='k',markersize=10) 139 | plt.xlim([np.min(x_list), np.max(x_list)]) 140 | 141 | #legend 142 | if legend_labels: 143 | plt.legend(legend_labels + ['sampled points']) 144 | 145 | def gen_sin_fig(agent, X,Y,freq,phase,amp,upper_x=5,lower_x=-5,point_every=0.1, label=None): 146 | y_list = [] 147 | x_list = [] 148 | s_list = [] 149 | for p in np.arange(lower_x,upper_x,0.1): 150 | y, s = agent.test(X, Y, [[[p]]]) 151 | y_list.append(y[0,0,0]) 152 | x_list.append(p) 153 | if s: 154 | s_list.append(s[0,0,0,0]) 155 | else: 156 | s_list.append(0) 157 | legend_labels = None 158 | if label: 159 | legend_labels = [label, 'True'] 160 | sinusoid_plot(freq,phase,amp,x_list,s_list,y_list,X,Y, legend_labels=legend_labels) 161 | 162 | def gen_sin_gp_fig(agent, X,Y,freq,phase,amp,upper_x=5,lower_x=-5,point_every=0.1, label=None): 163 | x_test = np.reshape( np.arange(lower_x,upper_x,0.1), [1,-1,1] ) 164 | y,s = agent.test(X,Y,x_test) 165 | y = y[0,:,0] 166 | s = s[0,:]**2 167 | legend_labels = None 168 | if label: 169 | legend_labels = [label, 'True'] 170 | sinusoid_plot(freq,phase,amp,x_test[0,:,0],s,y,X,Y,legend_labels=legend_labels) 171 | 172 | def plot_bases(x,y,indices): 173 | x = x[0,:,0] 174 | y = y[0,:,:] 175 | for i in indices: 176 | plt.figure() 177 | plt.plot(x,y[:,i]) 178 | plt.legend([r"$\phi_{"+ str(i) +r"}(x)$"]) 179 | plt.show() 180 | 181 | def gen_sin_bases_fig(agent, sess, x, n_bases): 182 | phi = sess.run( agent.phi, {agent.x: x} ) 183 | plot_bases(x, phi, np.random.choice(agent.config['nn_layers'][-1],n_bases)) 184 | 185 | 186 | def plot_sample_fns(x,phi,K,L,SigEps,n_samples): 187 | x = x[0,:,0] 188 | phi = phi[0,:,:] 189 | 190 | mean = np.reshape(K, [-1]) 191 | cov = np.kron(SigEps, np.linalg.inv(L)) 192 | K_vec = np.random.multivariate_normal(mean,cov,n_samples) 193 | plt.figure() 194 | for i in range(n_samples): 195 | K = np.reshape(K_vec[i,:], K.shape) 196 | y = np.squeeze(phi @ K) 197 | plt.plot(x,y) 198 | plt.show() 199 | 200 | # STEP FUNCTIONS 201 | def step_plot(x_jump,x_list,sigma_list,y_list,X_update, Y_update,sampling_density=101,legend_labels=['Ours', 'True']): 202 | """ 203 | x,y,sigma should be lists 204 | """ 205 | 206 | #plot given data 207 | conf_list = [1.96*np.sqrt(s) for s in sigma_list] 208 | upper = [y + c for y,c in zip(y_list,conf_list)] 209 | lower = [y - c for y,c in zip(y_list,conf_list)] 210 | plt.fill_between(x_list, upper, lower, alpha=.5) 211 | plt.plot(x_list,y_list) 212 | 213 | 214 | 215 | #plot true step 216 | yr_list = [0.5 + 0.5*np.sign(x-x_jump) for x in x_list] 217 | plt.plot(x_list,yr_list,color='r') 218 | 219 | # plot update points 220 | plt.plot(X_update[0,:,0],Y_update[0,:,0],'+',color='k',markersize=10) 221 | plt.xlim([np.min(x_list), np.max(x_list)]) 222 | plt.ylim([-1,2]) 223 | 224 | #legend 225 | if legend_labels: 226 | plt.legend(legend_labels + ['sampled points']) 227 | 228 | def multistep_plot(pt_list,x_list,sigma_list,y_list,X_update, Y_update,sampling_density=101,legend_labels=['Ours', 'True']): 229 | """ 230 | x,y,sigma should be lists 231 | """ 232 | 233 | #plot given data 234 | conf_list = [1.96*np.sqrt(s) for s in sigma_list] 235 | upper = [y + c for y,c in zip(y_list,conf_list)] 236 | lower = [y - c for y,c in zip(y_list,conf_list)] 237 | plt.fill_between(x_list, upper, lower, alpha=.5) 238 | plt.plot(x_list,y_list) 239 | 240 | 241 | 242 | #plot true step 243 | #yr_list = [] 244 | x = np.reshape(x_list,[1,-1]) 245 | step_pts = np.reshape(pt_list,[-1,1]) 246 | y = 2.*np.logical_xor.reduce( x > step_pts, axis=0) - 1. 247 | yr_list = y 248 | 249 | # for x in x_list: 250 | # for i in range(len(pt_list)): 251 | 252 | # if xpt_list[-1]: 257 | # # print('ok') 258 | # yr_list.append(((i+1)%2)*2-1.0) 259 | # break 260 | 261 | # if x>pt_list[i] and x 0 and not no_update: 399 | Kn,Ln_inv = agent.batch_update_np(K0,L0,uPhi[0,:,:],uy[0,:,:]) 400 | Ln = np.linalg.inv(Ln_inv) 401 | 402 | x = np.concatenate( ( X[0,0:1,:2], X[0,:T,:2] + Y[0,:T,:] ) ) 403 | x_pred = np.zeros([N_samples, T+1, X.shape[2]]) 404 | print(np.shape(x_pred[:,:Nu+1,:])) 405 | print(np.shape(x_pred)) 406 | print(np.shape(X[0:1, :Nu+1, :])) 407 | x_pred[:,:Nu+1,:] = X[0:1, :Nu+1, :] 408 | print(np.shape(x_pred)) 409 | for j in range(N_samples): 410 | K = sampleMN(Kn,Ln_inv,SigEps) 411 | # print(K) 412 | for t in range(Nu,Nu+T_rollout): 413 | phi_t = sess.run( agent.phi, {agent.x: x_pred[j:j+1, t:t+1, :]}) 414 | x_pred[j,t+1,:2] = x_pred[j,t,:2] + np.squeeze( phi_t[0,:,:] @ K ) 415 | 416 | dims = [0,1] 417 | colors = ['b','r'] 418 | styles=['-',':'] 419 | for i in dims: 420 | for j in range(N_samples): 421 | plt.plot(tt[Nu:Nu+T_rollout], x_pred[j,Nu:Nu+T_rollout,i], color=colors[i], alpha=5.0/N_samples) 422 | plt.plot(tt, x[:,i], linestyle=styles[i], color='k') 423 | 424 | ax = plt.gca() 425 | ylim = [np.min(x)-2,np.max(x)+2] 426 | #plt.plot([Nu, Nu],ylim,linestyle=':',color='k', alpha=0.8) 427 | #plt.fill_between([0,Nu],[ylim[1],ylim[1]],[ylim[0], ylim[0]],color='white',alpha=0.7,zorder=2) 428 | plt.xlim([tt[0],tt[-1]]) 429 | plt.ylim(ylim) 430 | #plt.show() 431 | 432 | 433 | def gen_pendulum_rollout_fig(agent, xu, xp, Nu, N_samples=50, T=None, T_rollout=10, update=True): 434 | if not T: 435 | T = xp.shape[1] 436 | 437 | tt = np.arange(T+1) 438 | 439 | x_dim = xp.shape[-1] 440 | x = xu[:,:,:x_dim] 441 | u = xu[:,:,x_dim:] 442 | 443 | agent.reset_to_prior() 444 | if update: 445 | for t in range(Nu): 446 | agent.incorporate_transition(x[0,t,:], u[0,t,:], xp[0,t,:]) 447 | 448 | Ks = agent.sample_dynamics_matrices(N_samples) 449 | x_pred = agent.sample_rollout(x[0,Nu,:],u[0,Nu:Nu+T_rollout,:],Ks) 450 | 451 | dims = [0,1] 452 | colors = ['b','r'] 453 | styles=['-',':'] 454 | for i in dims: 455 | for j in range(N_samples): 456 | plt.plot(tt[Nu:Nu+T_rollout+1], x_pred[j,:,i], color=colors[i], alpha=5.0/N_samples) 457 | plt.plot(tt, x[0,:T+1,i], linestyle=styles[i], color='k') 458 | 459 | ax = plt.gca() 460 | ylim = [np.min(x)-2,np.max(x)+2] 461 | plt.xlim([tt[0],tt[-1]]) 462 | plt.ylim(ylim) 463 | 464 | def gen_pendulum_sample_fig_gp(X,Y,Nu,N_samples=10,T=None, T_rollout=10): 465 | from sklearn.gaussian_process import GaussianProcessRegressor 466 | from sklearn.gaussian_process.kernels import RBF, ConstantKernel 467 | 468 | 2.5**2*RBF(0.5, (1e-1, 1e0)) 469 | 470 | if not T: 471 | T = Y.shape[1] 472 | 473 | tt = np.arange(T+1) 474 | 475 | 476 | 477 | kernel = 2.5**2*RBF(0.5, (1e-1, 1e0)) 478 | 479 | x = np.concatenate( ( X[0,0:1,:2], X[0,:T,:2] + Y[0,:T,:] ) ) 480 | x_pred = np.zeros([N_samples, T+1, X.shape[2]]) 481 | x_pred[:,:Nu+1,:] = X[0:1, :Nu+1, :] 482 | for j in range(N_samples): 483 | ux = X[0,:Nu,:] 484 | uy = Y[0,:Nu,:] 485 | for t in range(Nu,Nu+T_rollout): 486 | xt = x_pred[j,t,:] 487 | 488 | gp0 = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=0) 489 | if ux.shape[0] > 0: 490 | gp0.fit(ux, uy[:,0]) 491 | gp1 = GaussianProcessRegressor(kernel=kernel, n_restarts_optimizer=0) 492 | if ux.shape[0] > 0: 493 | gp1.fit(ux, uy[:,1]) 494 | 495 | y_samp = np.zeros((1, 2)) 496 | y_samp[0,0] = gp0.sample_y(x_pred[j,t:t+1,:],random_state=None) 497 | y_samp[0,1] = gp1.sample_y(x_pred[j,t:t+1,:],random_state=None) 498 | ux = np.concatenate( (ux, [xt]), axis=0 ) 499 | uy = np.concatenate( (uy, y_samp), axis=0 ) 500 | 501 | x_pred[j,t+1,:2] = xt[:2] + y_samp 502 | 503 | dims = [0,1] 504 | colors = ['b','r'] 505 | styles=['-',':'] 506 | for i in dims: 507 | for j in range(N_samples): 508 | plt.plot(tt[Nu:Nu+T_rollout], x_pred[j,Nu:Nu+T_rollout,i], color=colors[i], alpha=5.0/N_samples) 509 | plt.plot(tt, x[:,i], linestyle=styles[i], color='k') 510 | 511 | ax = plt.gca() 512 | ylim = [np.min(x)-2,np.max(x)+2] 513 | #plt.plot([Nu, Nu],ylim,linestyle=':',color='k', alpha=0.8) 514 | #plt.fill_between([0,Nu],[ylim[1],ylim[1]],[ylim[0], ylim[0]],color='white',alpha=0.7,zorder=2) 515 | plt.xlim([tt[0],tt[-1]]) 516 | plt.ylim(ylim) 517 | #plt.show() 518 | 519 | def test_adaptive_dynamics(agent, xu, xp, N_samples, Nu, T_rollout=30): 520 | agent.reset_to_prior() 521 | T = xp.shape[1] 522 | x_dim = xp.shape[2] 523 | u_dim = xu.shape[2] - x_dim 524 | 525 | tt = np.arange(T) 526 | 527 | u = xu[0,:,x_dim:] 528 | x = xu[0,:,:x_dim] 529 | 530 | for t in range(Nu): 531 | agent.incorporate_transition(x[t,:], u[t,:], xp[0,t,:]) 532 | 533 | Ks = agent.sample_dynamics_matrices(N_samples) 534 | x_pred = agent.sample_rollout(x[Nu,:], u[Nu:Nu+T_rollout,:], Ks) 535 | 536 | dims = range(x_dim) 537 | colors = ['b','r','g'] 538 | styles=['-',':','-.'] 539 | N_dims = len(dims) 540 | for i,d in enumerate(dims): 541 | plt.subplot(int( np.ceil( N_dims*1./2 )), 2, i+1) 542 | for j in range(N_samples): 543 | plt.plot(range(Nu, Nu+T_rollout+1), x_pred[j,:,d], color='C0', alpha=5.0/N_samples) 544 | plt.plot(tt, x[:T,d], linestyle=':', color='k') 545 | ylims = np.min(x[:,d]) + (np.max(x[:,d]) - np.min(x[:,d]))*np.array([-0.08, 1.08]) 546 | plt.ylabel(r"$x_{" + str(d+1) + r"}(k)$") 547 | plt.ylim(ylims) 548 | plt.xlim([tt[0],tt[-1]]) 549 | ax = plt.gca() 550 | #ax.yaxis.set_label_coords(-0.1, 0.5) 551 | if i % 2 == 1: 552 | ax.yaxis.tick_right() 553 | ax.yaxis.set_label_position("right") 554 | #ax.yaxis.set_label_coords(1.12, 0.5) 555 | if i >= N_dims - 2: 556 | plt.xlabel(r"$k$") 557 | else: 558 | plt.setp(ax.get_xticklabels(), visible=False) 559 | -------------------------------------------------------------------------------- /src/ALPaCA/dataset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3.6 2 | 3 | import numpy as np 4 | # import gym 5 | # import tqdm 6 | 7 | class Dataset: 8 | def __init__(self): 9 | pass 10 | 11 | # draw n_sample (x,y) pairs drawn from n_func functions 12 | # returns (x,y) where each has size [n_func, n_samples, x/y_dim] 13 | def sample(self, n_funcs, n_samples): 14 | raise NotImplementedError 15 | 16 | class PresampledDataset(Dataset): 17 | def __init__(self, X, Y): 18 | self.X = X 19 | self.Y = Y 20 | self.x_dim = X.shape[-1] 21 | self.y_dim = Y.shape[-1] 22 | self.N = X.shape[0] 23 | self.T = X.shape[1] 24 | 25 | def sample(self, n_funcs, n_samples): 26 | x = np.zeros((n_funcs, n_samples, self.x_dim)) 27 | y = np.zeros((n_funcs, n_samples, self.y_dim)) 28 | 29 | for i in range(n_funcs): 30 | j = np.random.randint(self.N) 31 | if n_samples > self.T: 32 | raise ValueError('You are requesting more samples than are in the dataset.') 33 | 34 | inds_to_keep = np.random.choice(self.T, n_samples) 35 | x[i,:,:] = self.X[j,inds_to_keep,:] 36 | y[i,:,:] = self.Y[j,inds_to_keep,:] 37 | 38 | return x,y 39 | 40 | class PresampledTrajectoryDataset(Dataset): 41 | def __init__(self, trajs, controls): 42 | self.trajs = trajs 43 | self.controls = controls 44 | self.o_dim = trajs[0].shape[-1] 45 | self.u_dim = controls[0].shape[-1] 46 | self.N = len(trajs) 47 | 48 | def sample(self, n_funcs, n_samples): 49 | o_dim = self.o_dim 50 | u_dim = self.u_dim 51 | x_dim = o_dim + u_dim 52 | y_dim = o_dim 53 | x = np.zeros((n_funcs, n_samples, x_dim)) 54 | y = np.zeros((n_funcs, n_samples, y_dim)) 55 | 56 | for i in range(n_funcs): 57 | j = np.random.randint(self.N) 58 | T = self.controls[j].shape[0] 59 | if n_samples > T: 60 | raise ValueError('You are requesting more samples than are in this trajectory.') 61 | start_ind = 0 62 | if T > n_samples: 63 | start_ind = np.random.randint(T-n_samples) 64 | inds_to_keep = np.arange(start_ind, start_ind+n_samples) 65 | x[i,:,:self.o_dim] = self.trajs[j][inds_to_keep] 66 | x[i,:,self.o_dim:] = self.controls[j][inds_to_keep] 67 | y[i,:,:] = self.trajs[j][inds_to_keep+1] #- self.trajs[j][inds_to_keep] 68 | 69 | return x,y 70 | 71 | class SinusoidDataset(Dataset): 72 | def __init__(self, config, noise_var=None, rng=None): 73 | self.amp_range = config['amp_range'] 74 | self.phase_range = config['phase_range'] 75 | self.freq_range = config['freq_range'] 76 | self.x_range = config['x_range'] 77 | if noise_var is None: 78 | self.noise_std = np.sqrt( config['sigma_eps'] ) 79 | else: 80 | self.noise_std = np.sqrt( noise_var ) 81 | 82 | self.np_random = rng 83 | if rng is None: 84 | self.np_random = np.random 85 | 86 | def sample(self, n_funcs, n_samples, return_lists=False): 87 | x_dim = 1 88 | y_dim = 1 89 | x = np.zeros((n_funcs, n_samples, x_dim)) 90 | y = np.zeros((n_funcs, n_samples, y_dim)) 91 | 92 | amp_list = self.amp_range[0] + self.np_random.rand(n_funcs)*(self.amp_range[1] - self.amp_range[0]) 93 | phase_list = self.phase_range[0] + self.np_random.rand(n_funcs)*(self.phase_range[1] - self.phase_range[0]) 94 | freq_list = self.freq_range[0] + self.np_random.rand(n_funcs)*(self.freq_range[1] - self.freq_range[0]) 95 | for i in range(n_funcs): 96 | x_samp = self.x_range[0] + self.np_random.rand(n_samples)*(self.x_range[1] - self.x_range[0]) 97 | y_samp = amp_list[i]*np.sin(freq_list[i]*x_samp + phase_list[i]) + self.noise_std*self.np_random.randn(n_samples) 98 | 99 | x[i,:,0] = x_samp 100 | y[i,:,0] = y_samp 101 | 102 | if return_lists: 103 | return x,y,freq_list,amp_list,phase_list 104 | 105 | return x,y 106 | 107 | class MultistepDataset(Dataset): 108 | def __init__(self, config, noise_var=None, rng=None): 109 | self.step_min = config['step_min'] 110 | self.step_max = config['step_max'] 111 | self.num_steps = config['num_steps'] 112 | self.x_range = config['x_range'] 113 | if noise_var is None: 114 | self.noise_std = np.sqrt( config['sigma_eps'] ) 115 | else: 116 | self.noise_std = np.sqrt( noise_var ) 117 | 118 | self.np_random = rng 119 | if rng is None: 120 | self.np_random = np.random 121 | 122 | def sample(self, n_funcs, n_samples, return_lists=False): 123 | x_dim = 1 124 | y_dim = 1 125 | x = np.zeros((n_funcs, n_samples, x_dim)) 126 | y = np.zeros((n_funcs, n_samples, y_dim)) 127 | 128 | step_mat = np.zeros((n_funcs, self.num_steps)) 129 | 130 | for i in range(n_funcs): 131 | step_pts = self.step_min + self.np_random.rand(self.num_steps)* (self.step_max - self.step_min) 132 | step_mat[i,:] = step_pts 133 | 134 | x_samp = self.x_range[0] + self.np_random.rand(n_samples)*(self.x_range[1] - self.x_range[0]) 135 | y_samp = self.multistep(x_samp, step_pts) 136 | 137 | x[i,:,0] = x_samp 138 | y[i,:,0] = y_samp 139 | 140 | if return_lists: 141 | return x,y,step_mat 142 | 143 | return x,y 144 | 145 | def multistep(self, x, step_pts): 146 | x = x.reshape([1,-1]) 147 | step_pts = step_pts.reshape([-1,1]) 148 | y = 2.*np.logical_xor.reduce( x > step_pts, axis=0) - 1. 149 | y += self.noise_std*self.np_random.randn(x.shape[1]) 150 | return y 151 | 152 | # # Assumes env has a forward_dynamics(x,u) function 153 | # class GymUniformSampleDataset(Dataset): 154 | # def __init__(self, env): 155 | # self.env = env 156 | # self.o_dim = env.observation_space.shape[-1] 157 | # self.u_dim = env.action_space.shape[-1] 158 | 159 | # def sample(self, n_funcs, n_samples): 160 | # o_dim = self.o_dim 161 | # u_dim = self.u_dim 162 | # x_dim = o_dim + u_dim 163 | # y_dim = o_dim 164 | # x = np.zeros((n_funcs, n_samples, x_dim)) 165 | # y = np.zeros((n_funcs, n_samples, y_dim)) 166 | 167 | # for i in range(n_funcs): 168 | # self.env.reset() 169 | # for j in range(n_samples): 170 | # s = self.env.get_ob_sample() 171 | # a = self.env.action_space.sample()#get_ac_sample() 172 | # sp = self.env.forward_dynamics(s,a) 173 | 174 | # x[i,j,:o_dim] = s 175 | # x[i,j,o_dim:] = a 176 | # y[i,j,:] = sp 177 | 178 | # return x,y 179 | 180 | # # wraps a gym env + policy as a dataset 181 | # # assumes that the gym env samples parameters from the prior upon reset 182 | # class GymDataset(Dataset): 183 | # def __init__(self, env, policy, state_dim=None): 184 | # self.env = gym.wrappers.TimeLimit(env, max_episode_steps=100) 185 | # self.policy = policy 186 | # self.use_state = False 187 | # self.o_dim = env.observation_space.shape[-1] 188 | # if state_dim is not None: 189 | # self.use_state = True 190 | # self.o_dim = state_dim 191 | # self.u_dim = env.action_space.shape[-1] 192 | 193 | # def sample(self, n_funcs, n_samples, shuffle=False, verbose=False): 194 | # o_dim = self.o_dim 195 | # u_dim = self.u_dim 196 | # x_dim = o_dim + u_dim 197 | # y_dim = o_dim 198 | # x = np.zeros((n_funcs, n_samples, x_dim)) 199 | # y = np.zeros((n_funcs, n_samples, y_dim)) 200 | 201 | 202 | # pbar = tqdm.tqdm(disable=(not verbose), total=n_funcs) 203 | # for i in range(n_funcs): 204 | # # sim a trajectory 205 | # x_traj = [] 206 | # u_traj = [] 207 | # xp_traj = [] 208 | 209 | # ob = self.env.reset() 210 | # if self.use_state: 211 | # s = self.env.unwrapped.state 212 | # done = False 213 | # while not done: 214 | # ac = self.policy(ob) 215 | # obp, _, done, _ = self.env.step(ac) 216 | 217 | # if self.use_state: 218 | # sp = self.env.unwrapped.state 219 | # x_traj.append(s) 220 | # u_traj.append(ac) 221 | # xp_traj.append(sp) 222 | # else: 223 | # x_traj.append(ob) 224 | # u_traj.append(ac) 225 | # xp_traj.append(obp) 226 | 227 | # ob = obp 228 | # if self.use_state: 229 | # s = sp 230 | 231 | # T = len(x_traj) 232 | # if T < n_samples: 233 | # print('episode did not last long enough') 234 | # #n_samples = T-1 235 | # i -= 1 236 | # continue 237 | 238 | # if shuffle: 239 | # inds_to_keep = np.random.choice(T, n_samples) 240 | # else: 241 | # start_ind = 0 #np.random.randint(T-n_samples) 242 | # inds_to_keep = range(start_ind, start_ind+n_samples) 243 | # x[i,:,:o_dim] = np.array(x_traj)[inds_to_keep,:] 244 | # x[i,:,o_dim:] = np.array(u_traj)[inds_to_keep,:] 245 | # y[i,:,:] = np.array(xp_traj)[inds_to_keep,:] 246 | 247 | # pbar.update(1) 248 | 249 | # pbar.close() 250 | # return x,y 251 | 252 | 253 | # class Randomizer(gym.Wrapper): 254 | # def __init__(self, env, prereset_fn): 255 | # super(Randomizer, self).__init__(env) 256 | # self.prereset_fn = prereset_fn 257 | 258 | # def reset(self): 259 | # self.prereset_fn(self.unwrapped) 260 | # return self.env.reset() 261 | -------------------------------------------------------------------------------- /src/adaptive_clbf.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import copy 3 | 4 | import rospy 5 | import actionlib 6 | import controller_adaptiveclbf.msg 7 | from controller_adaptiveclbf.srv import * 8 | 9 | from qp_solver import QPSolve 10 | from dynamics import DynamicsAckermannZ 11 | from cbf import BarrierAckermannVelocityZ, BarrierAckermannPointZ 12 | from lyapunov import LyapunovAckermannZ 13 | 14 | class AdaptiveClbf(object): 15 | def __init__(self,odim=2, use_service = True, ): 16 | self.xdim = 4 17 | self.udim = 2 18 | self.odim = odim 19 | self.use_service = use_service 20 | 21 | self.u_lim = np.array([[-2.0,2.0],[-1.0,1.0]]) 22 | self.K=np.block([[np.zeros((2,2)), np.zeros((2,2))],[np.eye(2), np.eye(2)]]) 23 | 24 | self.dyn = DynamicsAckermannZ() 25 | 26 | self.model_trained = False 27 | 28 | if self.use_service: 29 | ## train model action 30 | self.train_model_action_client = actionlib.SimpleActionClient('train_model_service', controller_adaptiveclbf.msg.TrainModelAction) 31 | self.train_model_action_client.wait_for_server() 32 | self.train_model_goal = controller_adaptiveclbf.msg.TrainModelGoal() 33 | 34 | ## add data srv 35 | rospy.wait_for_service('add_data_2_model') 36 | self.model_add_data_srv = rospy.ServiceProxy('add_data_2_model', AddData2Model) 37 | 38 | ## predict srv 39 | rospy.wait_for_service('predict_model') 40 | self.model_predict_srv = rospy.ServiceProxy('predict_model', PredictModel) 41 | else: 42 | # setup non-service model object 43 | from model_service import ModelVanillaService 44 | self.model = ModelVanillaService(self.xdim,self.odim,use_obs=True,use_service=False) 45 | 46 | # from model_service import ModelGPService 47 | # self.model = ModelGPService(self.xdim,self.odim,use_obs=True,use_service=False) 48 | 49 | # from model_service import ModelALPaCAService 50 | # self.model = ModelALPaCAService(self.xdim,self.odim,use_obs=True,use_service=False) 51 | 52 | self.clf = LyapunovAckermannZ(w1=10.0,w2=1.0,w3=1.0,epsilon=1.0) 53 | self.qpsolve = QPSolve(dyn=self.dyn,cbf_list=[],clf=self.clf,u_lim=self.u_lim,u_cost=0.0,u_prev_cost=1.0,p1_cost=1.0e8,p2_cost=1.0e8,verbose=False) 54 | 55 | x_init = np.zeros((self.xdim,1)) 56 | x_init[3] = 0.01 57 | self.z_ref = self.dyn.convert_x_to_z(x_init) 58 | self.z = copy.copy(self.z_ref) 59 | self.z_ref_dot = np.zeros((self.xdim,1)) 60 | self.z_dot = np.zeros((self.xdim/2,1)) 61 | self.z_prev = copy.copy(self.z) 62 | self.y_out = np.zeros((self.xdim/2,1)) 63 | self.mu_prev = np.zeros((self.xdim,1)) 64 | self.u_prev = np.zeros((self.udim,1)) 65 | self.u_prev_prev = np.zeros((self.udim,1)) 66 | self.obs_prev = np.zeros((self.odim,1)) 67 | self.dt = 0.1 68 | self.max_error = 1.0 69 | 70 | self.barrier_locations={} 71 | self.barrier_locations["x"]=np.array([]) 72 | self.barrier_locations["y"]=np.array([]) 73 | self.barrier_radius = 1.0 74 | 75 | self.measurement_noise = 1.0 76 | self.true_dyn = None 77 | self.true_predict_error = 0.0 78 | self.predict_error = 0 79 | self.predict_var = np.zeros((self.xdim/2,1)) 80 | 81 | self.debug={} 82 | 83 | def wrap_angle(self,a): 84 | return (a + np.pi) % (2 * np.pi) - np.pi 85 | 86 | def saturate(self,u,ulim): 87 | return np.array([np.clip(u[0],ulim[0][0],ulim[0][1]), np.clip(u[1],ulim[1][0],ulim[1][1])]) 88 | 89 | def update_params(self,params): 90 | self.params = params 91 | print("updated params!") 92 | self.vehicle_length = self.params["vehicle_length"] 93 | self.steering_limit = self.params["steering_limit"] 94 | self.max_accel = self.params["max_accel"] 95 | self.min_accel = self.params["min_accel"] 96 | self.u_lim = np.array([[np.tan(-self.steering_limit)/self.vehicle_length,np.tan(self.steering_limit)/self.vehicle_length], 97 | [self.min_accel,self.max_accel]],dtype=np.float32) 98 | self.qpsolve.u_lim = self.u_lim 99 | 100 | self.k1 = self.params["kp_z"] 101 | self.k2 = self.params["kd_z"] 102 | self.A=np.block([[np.zeros((2,2),dtype=np.float32), np.eye(2,dtype=np.float32)],[-self.k1*np.eye(2,dtype=np.float32), -self.k2*np.eye(2,dtype=np.float32)]]) 103 | self.qpsolve.update_ricatti(self.A) 104 | self.K=np.block([[self.k1*np.eye(2,dtype=np.float32), self.k2*np.eye(2,dtype=np.float32)]]) 105 | self.max_error = self.params["max_error"] 106 | 107 | self.clf.epsilon = self.params["clf_epsilon"] 108 | self.measurement_noise = self.params["measurement_noise"] 109 | 110 | self.qpsolve.u_cost = self.params["qp_u_cost"] 111 | self.qpsolve.u_prev_cost = self.params["qp_u_prev_cost"] 112 | self.qpsolve.p1_cost = self.params["qp_p1_cost"] 113 | self.qpsolve.p2_cost = self.params["qp_p2_cost"] 114 | self.qpsolve.verbose = self.params["qp_verbose"] 115 | self.qpsolve.ksig = self.params["qp_ksig"] 116 | self.qpsolve.max_var = self.params["qp_max_var"] 117 | self.verbose = self.params["verbose"] 118 | 119 | self.dt = self.params["dt"] 120 | 121 | # update model params if not using service calls 122 | if not self.use_service: 123 | self.model.N_data = self.params["N_data"] 124 | self.model.verbose = self.params["learning_verbose"] 125 | self.model.N_updates = self.params["N_updates"] 126 | self.model.config["meta_batch_size"] = self.params["meta_batch_size"] 127 | self.model.config["data_horizon"] = self.params["data_horizon"] 128 | self.model.config["test_horizon"] = self.params["test_horizon"] 129 | self.model.config["learning_rate"] = self.params["learning_rate"] 130 | self.model.config["min_datapoints"] = self.params["min_datapoints"] 131 | self.model.config["save_data_interval"] = self.params["save_data_interval"] 132 | 133 | def update_barrier_locations(self,x,y,radius): 134 | self.barrier_locations["x"] = x 135 | self.barrier_locations["y"] = y 136 | self.barrier_radius = radius 137 | 138 | def update_barriers(self): 139 | cbf_list = [] 140 | bar_loc_x = copy.copy(self.barrier_locations["x"]) 141 | bar_loc_y = copy.copy(self.barrier_locations["y"]) 142 | bar_rad = self.barrier_radius 143 | 144 | if self.params["use_barrier_vel"]: 145 | cbf_list = cbf_list + \ 146 | [BarrierAckermannVelocityZ(bound_from_above=True, v_lim = self.params["max_velocity"], gamma=self.params["barrier_vel_gamma"]), 147 | BarrierAckermannVelocityZ(bound_from_above=False, v_lim = self.params["min_velocity"], gamma=self.params["barrier_vel_gamma"])] 148 | 149 | if self.params["use_barrier_pointcloud"]: 150 | cbf_list = cbf_list + \ 151 | [BarrierAckermannPointZ(x=bar_loc_x[i],y=bar_loc_y[i], radius=bar_rad, gamma_p=self.params["barrier_pc_gamma_p"], gamma=self.params["barrier_pc_gamma"]) for i in range(bar_loc_x.size)] 152 | self.qpsolve.cbf_list = cbf_list 153 | 154 | def get_control(self,z,z_ref,z_ref_dot,dt,obs=None,train=False,use_model=False,add_data=True,check_model=True,use_qp=True): 155 | assert z.shape[0] == self.xdim + 1 156 | assert z_ref_dot.shape[0] == self.xdim + 1 157 | assert z_ref.shape[0] == self.xdim + 1 158 | 159 | self.update_barriers() 160 | 161 | self.z = copy.copy(z.astype(np.float32)) 162 | self.z_ref = copy.copy(z_ref.astype(np.float32)) 163 | self.obs = np.array(obs,dtype=np.float32) 164 | 165 | mu_ad = np.zeros((self.xdim/2,1),dtype=np.float32) 166 | mDelta = np.zeros((self.xdim/2,1),dtype=np.float32) 167 | sigDelta = np.zeros((self.xdim/2,1),dtype=np.float32) 168 | rho = np.zeros((self.xdim/2,1),dtype=np.float32) 169 | trueDelta = np.zeros((self.xdim/2,1),dtype=np.float32) 170 | 171 | e = self.z_ref[:-1,:]-self.z[:-1,:] 172 | mu_pd = np.matmul(self.K,e) 173 | mu_pd = np.clip(mu_pd,-self.max_error,self.max_error) 174 | 175 | # self.z_ref_dot = (self.z_ref_next[:-1,:] - self.z_ref[:-1,:]) / dt # use / self.dt for test_adaptive_clbf 176 | self.z_ref_dot = copy.copy(z_ref_dot) 177 | mu_rm = self.z_ref_dot[2:-1] 178 | 179 | mu_model = np.matmul(self.dyn.g(self.z_prev),self.u_prev) + self.dyn.f(self.z_prev) 180 | mu_model = np.clip(mu_model,-self.max_error,self.max_error) 181 | 182 | if add_data: 183 | if self.use_service: 184 | try: 185 | self.model_add_data_srv(self.z.flatten(),self.z_prev.flatten(),mu_model.flatten(),self.obs_prev.flatten(),dt) 186 | except: 187 | print("add data service unavailable") 188 | else: 189 | req = AddData2Model() 190 | req.x_next = self.z.flatten() 191 | req.x = self.z_prev.flatten() 192 | req.mu_model = mu_model.flatten() 193 | req.obs = self.obs_prev.flatten() 194 | req.dt = dt 195 | self.model.add_data(req) 196 | 197 | self.z_dot = (self.z[2:-1,:]-self.z_prev[2:-1,:])/dt - mu_model 198 | 199 | # if check_model and self.model.model_trained: 200 | if check_model and self.model_trained: 201 | # check how the model is doing. compare the model's prediction with the actual sampled data. 202 | predict_service_success = False 203 | result = None 204 | if self.use_service: 205 | try: 206 | result = self.model_predict_srv(self.z_prev.flatten(),self.obs_prev.flatten()) 207 | if result.result: 208 | predict_service_success = True 209 | except: 210 | print("predict service unavailable") 211 | else: 212 | req = PredictModel() 213 | req.x = self.z_prev.flatten() 214 | req.obs = self.obs_prev.flatten() 215 | result = self.model.predict(req) 216 | predict_service_success = True 217 | 218 | if predict_service_success: 219 | self.y_out = np.expand_dims(result.y_out, axis=0).T 220 | var = np.expand_dims(result.var, axis=0).T 221 | 222 | if self.verbose: 223 | print("predicted y_out: ", self.y_out) 224 | print("predicted ynew: ", self.z_dot) 225 | print("predicted var: ", var) 226 | 227 | self.predict_error = np.linalg.norm(self.y_out - self.z_dot) 228 | self.predict_var = var 229 | 230 | if use_model and self.model_trained: 231 | predict_service_success = False 232 | result = None 233 | if self.use_service: 234 | try: 235 | result = self.model_predict_srv(self.z.flatten(),self.obs.flatten()) 236 | if result.result: 237 | predict_service_success = True 238 | except: 239 | print("predict service unavailable") 240 | else: 241 | req = PredictModel() 242 | req.x = self.z.flatten() 243 | req.obs = self.obs.flatten() 244 | result = self.model.predict(req) 245 | predict_service_success = True 246 | 247 | if predict_service_success: 248 | mDelta = np.expand_dims(result.y_out, axis=0).T 249 | sigDelta = np.expand_dims(result.var, axis=0).T 250 | 251 | # log error if true system model is available 252 | if self.true_dyn is not None: 253 | trueDelta = self.true_dyn.f(self.z) - self.dyn.f(self.z) 254 | self.true_predict_error = np.linalg.norm(trueDelta - mDelta) 255 | 256 | # rho = self.measurement_noise / (self.measurement_noise + (sigDelta - 1.0) + 1e-6) 257 | rho[0] = self.measurement_noise / (self.measurement_noise + np.linalg.norm(sigDelta) + 1e-6) 258 | #rho = np.linalg.norm(sigDelta) * self.measurement_noise 259 | mu_ad = mDelta * rho[0] 260 | sigDelta = sigDelta * (1.0 - rho[0]) 261 | else: 262 | sigDelta = np.ones((self.xdim/2,1)) 263 | 264 | mu_d = mu_rm + mu_pd - mu_ad 265 | self.mu_qp = np.zeros((self.xdim/2,1)) 266 | if use_qp: 267 | self.mu_qp = self.qpsolve.solve(self.z,self.z_ref,mu_d,sigDelta) 268 | 269 | self.mu_new = mu_d + self.mu_qp 270 | self.u_new = np.matmul(np.linalg.inv(self.dyn.g(self.z)), (self.mu_new-self.dyn.f(self.z))) 271 | 272 | u_new_unsaturated = copy.copy(self.u_new) 273 | #saturate in case constraints are not met 274 | self.u_new = self.saturate(self.u_new,self.u_lim) 275 | 276 | if self.verbose: 277 | print('z: ', self.z.T) 278 | print('z_ref: ', self.z_ref.T) 279 | print('mu_rm', mu_rm) 280 | print('mu_pd', mu_pd) 281 | print('mu_ad', mu_ad) 282 | print('mu_d', mu_d) 283 | print('mu_model', mu_model) 284 | print('rho', rho) 285 | print('mu_qp', self.mu_qp) 286 | print('mu',self.mu_new) 287 | print('u_new', self.u_new) 288 | print('u_unsat', u_new_unsaturated) 289 | print('trueDelta',trueDelta) 290 | print('true predict error', self.true_predict_error) 291 | print('mDelta', mDelta) 292 | print('sigDelta', sigDelta) 293 | 294 | self.debug["z"] = self.z.flatten().tolist() 295 | self.debug["z_ref"] = self.z_ref.flatten().tolist() 296 | self.debug["z_dot"] = self.z_dot.flatten().tolist() 297 | self.debug["y_out"] = self.y_out.flatten().tolist() 298 | self.debug["mu_rm"] = self.z_ref_dot.flatten().tolist() 299 | self.debug["mu_pd"] = mu_pd.flatten().tolist() 300 | self.debug["mu_ad"] = mu_ad.flatten().tolist() 301 | self.debug["mu_model"] = mu_model.flatten().tolist() 302 | self.debug["rho"] = rho.flatten().tolist() 303 | self.debug["mu_qp"] = self.mu_qp.flatten().tolist() 304 | self.debug["mu"] = self.mu_new.flatten().tolist() 305 | self.debug["u_new"] = self.u_new.flatten().tolist() 306 | self.debug["u_unsat"] = u_new_unsaturated.flatten().tolist() 307 | self.debug["trueDelta"] = trueDelta.flatten().tolist() 308 | self.debug["true_predict_error"] = self.true_predict_error 309 | self.debug["mDelta"] = mDelta.flatten().tolist() 310 | self.debug["sigDelta"] = sigDelta.flatten().tolist() 311 | 312 | self.mu_prev = copy.copy(self.mu_new) 313 | self.u_prev_prev = copy.copy(self.u_prev) 314 | self.u_prev = copy.copy(self.u_new) 315 | self.obs_prev = copy.copy(self.obs) 316 | self.z_prev = copy.copy(self.z) 317 | 318 | self.controls = np.zeros(self.udim) 319 | self.controls[0] = np.arctan(self.u_new[0] * self.vehicle_length) 320 | self.controls[1] = self.u_new[1] 321 | return self.controls 322 | -------------------------------------------------------------------------------- /src/bag_data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import rosbag 6 | 7 | from dynamic_reconfigure.client import Client as DynamicReconfigureClient 8 | 9 | from nav_msgs.msg import Odometry 10 | from std_msgs.msg import Bool, Time 11 | 12 | import os 13 | import time 14 | 15 | BASE_PATH = '/home/nereid/learn2adapt_ws/' 16 | BAG_FILENAME = 'test' 17 | 18 | class BagDataNode(object): 19 | def __init__(self): 20 | self.bag = rosbag.Bag(BASE_PATH + BAG_FILENAME + '.bag', 'w') 21 | 22 | # dynamic reconfigure client 23 | self.dyn_reconfig_client = DynamicReconfigureClient('controller_adaptiveclbf_reconfig', timeout=30, config_callback=self.reconfigure_cb) 24 | # for no learning uncomment below 25 | # self.dyn_reconfig_client.update_configuration({"model_train": False}) 26 | 27 | self.sub_odom = rospy.Subscriber('/downward/vio/odometry', Odometry, self.odom_cb, queue_size=1) 28 | self.sub_ref = rospy.Subscriber('/reference_vis', Odometry, self.ref_cb, queue_size=1) 29 | 30 | self.odom_msg = Odometry() 31 | self.ref_msg = Odometry() 32 | 33 | self.start_time = rospy.Time.now() 34 | self.use_model = False 35 | 36 | def reconfigure_cb(self, config): 37 | """Create a callback function for the dynamic reconfigure client.""" 38 | 39 | def odom_cb(self, odom_msg): 40 | if self.start_time == 0: 41 | self.start_time = rospy.Time.now() 42 | 43 | self.odom_msg = odom_msg 44 | 45 | 46 | def ref_cb(self, odom_msg): 47 | self.ref_msg = odom_msg 48 | 49 | if __name__ == '__main__': 50 | rospy.init_node('bag_data_node') 51 | node = BagDataNode() 52 | 53 | time_elapsed = Time() 54 | 55 | rate = rospy.Rate(10) 56 | while not rospy.is_shutdown(): 57 | curr_time = rospy.Time.now() 58 | 59 | if curr_time.to_sec() - node.start_time.to_sec() > 1e-4 and not node.use_model and curr_time.to_sec() - node.start_time.to_sec() >= 30: # 30 seconds 60 | node.dyn_reconfig_client.update_configuration({"use_model": True}) 61 | node.use_model = True 62 | print("------- Using model -------") 63 | 64 | if curr_time.to_sec() - node.start_time.to_sec() > 1e-4 and curr_time.to_sec() - node.start_time.to_sec() >= 120: 65 | node.bag.close() 66 | os.system("rosnode kill -a && sleep 5 && kill -2 $( ps -C roslaunch -o pid= ) && sleep 2 && kill -2 $( ps -C roscore -o pid= )") 67 | os.system("tmux kill-session") 68 | break 69 | 70 | rate.sleep() 71 | node.bag.write("/downward/vio/odometry", node.odom_msg) 72 | node.bag.write("/reference_vis", node.ref_msg) 73 | time_elapsed.data = curr_time - node.start_time 74 | node.bag.write("/time_elapsed", time_elapsed) 75 | 76 | print("Time elapsed", time_elapsed.data.to_sec()) 77 | -------------------------------------------------------------------------------- /src/cbf.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Barrier(): 4 | def __init__(self,dim,gamma): 5 | self.dim=dim 6 | self.gamma = gamma 7 | 8 | def h(self,x): 9 | # overload 10 | return None 11 | 12 | def dh(self,x): 13 | # overload 14 | return None 15 | 16 | def B(self,x): 17 | # TODO: parameterize this, and craft something better. 18 | hx = self.h(x) 19 | # return np.exp(-hx+1) 20 | if hx == 0: 21 | hx = 1e-3 22 | return 1.0/hx 23 | 24 | def dB(self,x): 25 | hx = self.h(x) 26 | if hx == 0: 27 | hx = 1e-3 28 | return -1.0/(hx**2)*self.dh(x) 29 | # return -np.exp(-hx+1)*self.dh(x) 30 | 31 | def d2B(self,x): 32 | hx = self.h(x) 33 | if hx == 0: 34 | hx = 1e-3 35 | # return -1.0/(hx**3)*(self.d2h(x) -2*np.matmul(self.dh(x),self.dh(x).T)) 36 | # can ignore d2h because taking trace(G*sig*sig^T*G^T d2B). 37 | dh = self.dh(x)[2:].T 38 | return 1.0/(hx**3)*(2*np.outer(dh,dh)) 39 | 40 | def get_B_derivatives(self,x): 41 | hx = self.h(x) 42 | if hx == 0: 43 | hx = 1e-3 44 | 45 | dh = self.dh(x) 46 | return hx, -1.0/(hx*hx)*dh.T, 2.0/(hx*hx*hx)*(np.outer(dh[2:],dh[2:])) 47 | 48 | 49 | class BarrierAckermannVelocity(Barrier): 50 | def __init__(self,dim=4,gamma=1.0,bound_from_above=True, v_lim = 1.0): 51 | self.bound_from_above = bound_from_above 52 | self.v_lim = v_lim 53 | 54 | Barrier.__init__(self,dim,gamma) 55 | 56 | def h(self,x): 57 | sgn = 1.0 58 | if self.bound_from_above: 59 | sgn = -1.0 60 | return sgn * (x[3,:] - self.v_lim) 61 | 62 | def dh(self,x): 63 | sgn = 1.0 64 | if self.bound_from_above: 65 | sgn = -1.0 66 | return np.array([[0],[0],[0],[sgn]]) 67 | 68 | def d2h(self,x): 69 | return np.array([[0],[0],[0],[0]]) 70 | 71 | class BarrierAckermannPosition(Barrier): 72 | def __init__(self,dim=4,gamma=1.0, bound_from_above = True, bound_x = True, pos_lim = 1.0, gamma_p = 1.0): 73 | self.bound_from_above = bound_from_above 74 | self.bound_x = bound_x 75 | self.pos_lim = pos_lim 76 | self.gamma_p = gamma_p 77 | 78 | Barrier.__init__(self,dim,gamma) 79 | 80 | def h(self,x): 81 | sgn = 1.0 82 | if self.bound_from_above: 83 | sgn = -1.0 84 | 85 | if self.bound_x: 86 | return sgn * (self.gamma_p * (x[0,:] - self.pos_lim) + x[3,:] * np.cos(x[2,:])) 87 | else: 88 | return sgn * (self.gamma_p * (x[1,:] - self.pos_lim) + x[3,:] * np.sin(x[2,:])) 89 | 90 | def dh(self,x): 91 | sgn = 1.0 92 | if self.bound_from_above: 93 | sgn = -1.0 94 | 95 | if self.bound_x: 96 | return sgn * np.stack((np.array([self.gamma_p]),np.array([0]),-x[3,:] * np.sin(x[2,:]),np.cos(x[2,:])),axis=0) 97 | else: 98 | return sgn * np.stack((np.array([0]),np.array([self.gamma_p]),x[3,:] * np.cos(x[2,:]),np.sin(x[2,:])),axis=0) 99 | 100 | class BarrierAckermannPositionZ(Barrier): 101 | def __init__(self,dim=4,gamma=1.0, bound_from_above = True, bound_x = True, pos_lim = 1.0, gamma_p = 1.0): 102 | self.bound_from_above = bound_from_above 103 | self.bound_x = bound_x 104 | self.pos_lim = pos_lim 105 | self.gamma_p = gamma_p 106 | 107 | Barrier.__init__(self,dim,gamma) 108 | 109 | def h(self,z): 110 | sgn = 1.0 111 | if self.bound_from_above: 112 | sgn = -1.0 113 | 114 | if self.bound_x: 115 | return sgn * (self.gamma_p * (z[0,:] - self.pos_lim) + z[2,:]) 116 | else: 117 | return sgn * (self.gamma_p * (z[1,:] - self.pos_lim) + z[3,:]) 118 | 119 | def dh(self,z): 120 | sgn = 1.0 121 | if self.bound_from_above: 122 | sgn = -1.0 123 | 124 | if self.bound_x: 125 | return np.array([[sgn*self.gamma_p],[0],[sgn],[0]]) 126 | else: 127 | return np.array([[0],[sgn*self.gamma_p],[0],[sgn]]) 128 | 129 | class BarrierAckermannVelocityZ(Barrier): 130 | def __init__(self,dim=4,gamma=1.0,bound_from_above=True, v_lim = 1.0): 131 | self.bound_from_above = bound_from_above 132 | self.v_lim = v_lim 133 | 134 | Barrier.__init__(self,dim,gamma) 135 | 136 | def h(self,z): 137 | sgn = 1.0 138 | if self.bound_from_above: 139 | sgn = -1.0 140 | return sgn * (np.sqrt(z[2]**2 + z[3]**2) * z[4] - self.v_lim) 141 | 142 | def dh(self,z): 143 | sgn = 1.0 144 | if self.bound_from_above: 145 | sgn = -1.0 146 | z1 = z[0,:] 147 | z2 = z[1,:] 148 | z3 = z[2,:] 149 | z4 = z[3,:] 150 | z5 = z[4,:] 151 | v = (np.sqrt(z3**2 + z4**2) + 1.0e-6) 152 | return np.stack((np.array([0]), np.array([0]), (z3*z5)/v, (z4*z5)/v)) * sgn 153 | 154 | def d2h(self,z): 155 | sgn = 1.0 156 | if self.bound_from_above: 157 | sgn = -1.0 158 | z1 = z[0,:] 159 | z2 = z[1,:] 160 | z3 = z[2,:] 161 | z4 = z[3,:] 162 | z5 = z[4,:] 163 | v = (np.sqrt(z3**2 + z4**2) + 1.0e-6) 164 | H = np.block([[(z4**2*z5)/(v**3), -(z3*z4*z5)/(v**3)], 165 | [-(z3*z4*z5)/(v**3), (z3**2*z5)/(v**3)]]) 166 | return np.block([[np.zeros((2,2)),np.zeros((2,2))],[np.zeros((2,2)),H]], dtype=np.float32) * sgn 167 | 168 | class BarrierAckermannPointZ(Barrier): 169 | def __init__(self,dim=4,gamma=1.0, x=0.0, y=0.0, radius = 1.0, gamma_p = 1.0): 170 | self.x = x 171 | self.y = y 172 | self.radius = radius 173 | self.gamma_p = gamma_p 174 | 175 | Barrier.__init__(self,dim,gamma) 176 | 177 | def h(self,z): 178 | sgn = 1.0 179 | if self.radius < 0.0: 180 | sgn = -1.0 181 | 182 | d = np.sqrt((z[0] - self.x)*(z[0] - self.x) + (z[1] - self.y)*(z[1] - self.y)) + 1.0e-6 183 | return sgn * (self.gamma_p * (d - self.radius) + (z[0] - self.x) / d * z[2] + (z[1] - self.y) / d * z[3]) 184 | 185 | def dh(self,z): 186 | sgn = 1.0 187 | if self.radius < 0.0: 188 | sgn = -1.0 189 | 190 | d = np.sqrt((z[0] - self.x)*(z[0] - self.x) + (z[1] - self.y)*(z[1] - self.y)) + 1.0e-6 191 | d_2 = d*d 192 | d_3 = d*d*d 193 | y_pos_m_z2 = (self.y - z[1]) 194 | x_pos_m_z1 = (self.x - z[0]) 195 | return sgn * np.array(( 196 | (z[2]*(d_2 - x_pos_m_z1*x_pos_m_z1) - self.gamma_p * d_2 *x_pos_m_z1 - z[3]*x_pos_m_z1*y_pos_m_z2)/d_3, 197 | (z[3]*(d_2 - y_pos_m_z2*y_pos_m_z2) - self.gamma_p * d_2 *y_pos_m_z2 - z[2]*x_pos_m_z1*y_pos_m_z2)/d_3, 198 | -x_pos_m_z1/d, 199 | -y_pos_m_z2/d), dtype=np.float32) 200 | 201 | def d2h(self,z): 202 | sgn = 1.0 203 | if self.radius < 0.0: 204 | sgn = -1.0 205 | 206 | d = np.sqrt((z[0,:] - self.x)**2 + (z[1,:] - self.y)**2) + 1.0e-6 207 | z1 = z[0,:] 208 | z2 = z[1,:] 209 | z3 = z[2,:] 210 | z4 = z[3,:] 211 | z5 = z[4,:] 212 | gamma_p = self.gamma_p 213 | x_pos = self.x 214 | y_pos = self.y 215 | y_pos_m_z2 = (y_pos - z2) 216 | x_pos_m_z1 = (x_pos - z1) 217 | d2 = d**2 218 | d3 = d**3 219 | d5 = d**5 220 | z1_2 = z1**2 221 | z2_2 = z2**2 222 | x_pos_2 = x_pos**2 223 | y_pos_2 = y_pos**2 224 | a11 = (y_pos_m_z2*(gamma_p *(x_pos_2*y_pos - x_pos_2*z2 + 3*y_pos*z2_2 - 2*x_pos*y_pos*z1 + 2*x_pos*z1*z2 + y_pos**3 - 3*y_pos_2*z2 + y_pos*z1_2 - z1_2*z2 - z2**3) - 2*z4*x_pos_2 + 3*z3*x_pos*y_pos + 4*z4*x_pos*z1 - 3*z3*x_pos*z2 + z4*y_pos_2 - 3*z3*y_pos*z1 - 2*z4*y_pos*z2 - 2*z4*z1_2 + 3*z3*z1*z2 + z4*z2_2))/(d5) 225 | a12 = x_pos_m_z1 * y_pos_m_z2 * (z4 + z3 - gamma_p - (3*z3*x_pos_m_z1)/(d2) - (3*z4*y_pos_m_z2)/(d2))/(d3) 226 | a13 = y_pos_m_z2**2/(d3) 227 | a14 = -(x_pos_m_z1*y_pos_m_z2)/(d3) 228 | a22 = (x_pos_m_z1*(gamma_p*(x_pos**3 - 3*x_pos_2*z1 + x_pos*y_pos_2 - 2*x_pos*y_pos*z2 + 3*x_pos*z1_2 + x_pos*z2_2 - y_pos_2*z1 + 2*y_pos*z1*z2 - z1**3 - z1*z2_2)+ z3*x_pos_2 + 3*z4*x_pos*y_pos - 2*z3*x_pos*z1 - 3*z4*x_pos*z2 - 2*z3*y_pos_2 - 3*z4*y_pos*z1 + 4*z3*y_pos*z2 + z3*z1_2 + 3*z4*z1*z2 - 2*z3*z2_2))/(d5) 229 | a23 = -(y_pos_m_z2*x_pos_m_z1)/(d3) 230 | a24 = x_pos_m_z1**2/(d3) 231 | 232 | return sgn * np.block([ 233 | [ a11, a12, a13, a14], 234 | [ a12, a22, a23, a24], 235 | [ a13, a23, 0, 0], 236 | [ a14, a24, 0, 0]], dtype=np.float32) -------------------------------------------------------------------------------- /src/dyn_reconfig_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | 4 | from dynamic_reconfigure.server import Server as DynamicReconfigureServer 5 | from controller_adaptiveclbf.cfg import adaptiveClbfConfig as ConfigType 6 | 7 | def reconfigure_cb(config, dummy): 8 | """Create a callback function for the dynamic reconfigure server.""" 9 | return config 10 | 11 | if __name__ == "__main__": 12 | rospy.init_node('controller_adaptiveclbf_reconfig') 13 | 14 | # Create a dynamic reconfigure server. 15 | server = DynamicReconfigureServer(ConfigType, reconfigure_cb) 16 | rospy.spin() 17 | -------------------------------------------------------------------------------- /src/dynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | class Dynamics(): 5 | def __init__(self,xdim,udim): 6 | self.xdim=xdim 7 | self.udim=udim 8 | 9 | def f(self,x): 10 | # overload 11 | return None 12 | 13 | def g(self,x): 14 | # overload 15 | return None 16 | 17 | def pseudoinv(self,x): 18 | return np.matmul(np.linalg.inv(np.matmul(x.T,x)),x.T) 19 | 20 | def convert_z_to_x(self,z): 21 | v=np.sqrt(z[2,:]**2 + z[3,:]**2) * z[4,:] 22 | return np.stack((z[0,:],z[1,:],np.arctan2(z[4,:]*z[3,:],z[4,:]*z[2,:]),v),axis=0) 23 | 24 | def convert_x_to_z(self,x): 25 | v_sign = 1.0 26 | if x[3,:] < 0: 27 | v_sign = -1 28 | return np.stack((x[0,:],x[1,:],x[3,:]*np.cos(x[2,:]),x[3,:]*np.sin(x[2,:]),np.array([v_sign])),axis=0) 29 | 30 | class DynamicsAckermann(Dynamics): 31 | def __init__(self,xdim=4,udim=2): 32 | Dynamics.__init__(self,xdim=xdim,udim=udim) 33 | 34 | def f(self,x): 35 | return np.stack((x[3,:] * np.cos(x[2,:]), x[3,:] * np.sin(x[2,:]), np.array([0]), np.array([0]))) 36 | 37 | def g(self,x): 38 | return np.stack((np.array([0,0]),np.array([0,0]),np.append(x[3,:],0),np.array([0,1]))) 39 | 40 | class DynamicsAckermannModified(Dynamics): 41 | def __init__(self,xdim=4,udim=2): 42 | Dynamics.__init__(self,xdim=xdim,udim=udim) 43 | 44 | def f(self,x): 45 | v_modified = x[3,:]**2 / (2*np.tanh(x[3,:])) 46 | return np.stack((v_modified * np.cos(x[2,:]), v_modified * np.sin(x[2,:]), x[3,:], -0.5*x[3,:])) 47 | 48 | def g(self,x): 49 | return np.stack((np.array([0,0]),np.array([0,0]),np.append(x[3,:],0),np.array([0,1])))*1.2 50 | 51 | 52 | class DynamicsAckermannZ(Dynamics): 53 | def __init__(self,xdim=4,udim=2,epsilon=1e-6): 54 | self.epsilon = epsilon 55 | Dynamics.__init__(self,xdim=xdim,udim=udim) 56 | 57 | def f(self,z): 58 | return np.stack((np.array([0]),np.array([0]))) * z[4,:] 59 | 60 | def g(self,z): 61 | v=(np.sqrt(z[2,:]**2 + z[3,:]**2) + self.epsilon) * z[4,:] 62 | return np.stack((np.concatenate((-z[3,:]*v,z[2,:]/v)),np.concatenate((z[2,:]*v,z[3,:]/v)))) 63 | 64 | 65 | class DynamicsAckermannZModified(Dynamics): 66 | def __init__(self,xdim=4,udim=2,epsilon=1e-6,disturbance_scale_pos = 0.5,disturbance_scale_vel = 2.0,control_input_scale = 2.0): 67 | self.epsilon = epsilon 68 | Dynamics.__init__(self,xdim=xdim,udim=udim) 69 | 70 | self.disturbance_scale_pos = disturbance_scale_pos 71 | self.disturbance_scale_vel = disturbance_scale_vel 72 | self.control_input_scale = control_input_scale 73 | 74 | def f(self,z): 75 | v=np.sqrt(z[2,:]**2 + z[3,:]**2) * z[4,:] 76 | theta = np.arctan2(z[3]*z[4],z[2]*z[4]) 77 | v_disturbance_body = [np.tanh(v**2)*self.disturbance_scale_vel, (0.1+v)*self.disturbance_scale_vel] 78 | v_disturbance_world = [v_disturbance_body[0] * np.cos(theta) - v_disturbance_body[1] * np.sin(theta), 79 | v_disturbance_body[0] * np.sin(theta) + v_disturbance_body[1] * np.cos(theta)] 80 | return np.array([v_disturbance_world[0], v_disturbance_world[1]]) 81 | 82 | def g(self,z): 83 | v=(np.sqrt(z[2,:]**2 + z[3,:]**2) + self.epsilon) * z[4,:] 84 | return self.control_input_scale * np.stack((np.concatenate((-z[3,:]*v,z[2,:]/v)),np.concatenate((z[2,:]*v,z[3,:]/v)))) 85 | 86 | def step(self,z,u,dt): 87 | v=np.sqrt(z[2,:]**2 + z[3,:]**2) 88 | znext = np.zeros((self.xdim+1,1)) 89 | znext[0:2,:] = z[0:2,:] + dt*(z[2:-1,:] + np.array([np.sin(v**3)*self.disturbance_scale_pos, np.cos(-v)*self.disturbance_scale_pos])) 90 | znext[2:-1,:] = z[2:-1,:] + dt*(self.f(z) + np.matmul(self.g(z),u)) 91 | znext[-1] = z[-1] 92 | return znext 93 | -------------------------------------------------------------------------------- /src/encoder_odometry.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | diff_tf.py - follows the output of a wheel encoder and 5 | creates tf and odometry messages. 6 | some code borrowed from the arbotix diff_controller script 7 | A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ 8 | 9 | Copyright (C) 2012 Jon Stephan. 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | 24 | ---------------------------------- 25 | Portions of this code borrowed from the arbotix_python diff_controller. 26 | 27 | diff_controller.py - controller for a differential drive 28 | Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved. 29 | 30 | Redistribution and use in source and binary forms, with or without 31 | modification, are permitted provided that the following conditions are met: 32 | * Redistributions of source code must retain the above copyright 33 | notice, this list of conditions and the following disclaimer. 34 | * Redistributions in binary form must reproduce the above copyright 35 | notice, this list of conditions and the following disclaimer in the 36 | documentation and/or other materials provided with the distribution. 37 | * Neither the name of Vanadium Labs LLC nor the names of its 38 | contributors may be used to endorse or promote products derived 39 | from this software without specific prior written permission. 40 | 41 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 42 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 43 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 44 | DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, 45 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 46 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 47 | OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 48 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 49 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 50 | ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 51 | 52 | """ 53 | 54 | import rospy 55 | import roslib 56 | from math import sin, cos, pi 57 | 58 | from geometry_msgs.msg import Quaternion 59 | from geometry_msgs.msg import Twist 60 | from nav_msgs.msg import Odometry 61 | from tf.broadcaster import TransformBroadcaster 62 | from std_msgs.msg import Int32 63 | 64 | ############################################################################# 65 | class DiffTf: 66 | ############################################################################# 67 | 68 | ############################################################################# 69 | def __init__(self): 70 | ############################################################################# 71 | rospy.init_node("encoder_odometry") 72 | self.nodename = rospy.get_name() 73 | rospy.loginfo("-I- %s started" % self.nodename) 74 | 75 | #### parameters ####### 76 | self.rate = rospy.get_param('~rate',50.0) # the rate at which to publish the transform 77 | self.ticks_meter = float(rospy.get_param('ticks_meter', 12.0*40.0/(2.0*pi*0.05))) # The number of wheel encoder ticks per meter of travel 78 | self.base_width = float(rospy.get_param('~base_width', 0.2)) # The wheel base width in meters 79 | 80 | self.base_frame_id = rospy.get_param('~base_frame_id','base_link') # the name of the base frame of the robot 81 | self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame 82 | 83 | self.encoder_min = rospy.get_param('encoder_min', -32768) 84 | self.encoder_max = rospy.get_param('encoder_max', 32768) 85 | self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) 86 | self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) 87 | 88 | # internal data 89 | self.enc_fleft = None # wheel encoder readings 90 | self.enc_fright = None 91 | self.enc_rleft = None 92 | self.enc_rright = None 93 | self.fleft = 0 # actual values coming back from robot 94 | self.fright = 0 95 | self.rleft = 0 96 | self.rright = 0 97 | 98 | self.flmult = 0 99 | self.frmult = 0 100 | self.rlmult = 0 101 | self.rrmult = 0 102 | 103 | self.prev_flencoder = 0 104 | self.prev_frencoder = 0 105 | self.prev_rlencoder = 0 106 | self.prev_rrencoder = 0 107 | 108 | self.d_fleft = 0 109 | self.d_fright = 0 110 | self.d_rleft = 0 111 | self.d_rright = 0 112 | 113 | self.x = 0 # position in xy plane 114 | self.y = 0 115 | self.th = 0 116 | self.dx = 0 # speeds in x/rotation 117 | self.dr = 0 118 | self.past_time = rospy.Time.now() 119 | 120 | # subscriptions 121 | rospy.Subscriber("/encoder/front_left", Int32, self.flwheelCallback) 122 | rospy.Subscriber("/encoder/front_right", Int32, self.frwheelCallback) 123 | rospy.Subscriber("/encoder/rear_left", Int32, self.rlwheelCallback) 124 | rospy.Subscriber("/encoder/rear_right", Int32, self.rrwheelCallback) 125 | self.odomPub = rospy.Publisher("encoder/odom", Odometry, queue_size=10) 126 | # self.odomBroadcaster = TransformBroadcaster() 127 | 128 | ############################################################################# 129 | def spin(self): 130 | ############################################################################# 131 | r = rospy.Rate(self.rate) 132 | while not rospy.is_shutdown(): 133 | r.sleep() 134 | self.update() 135 | 136 | 137 | ############################################################################# 138 | def update(self): 139 | ############################################################################# 140 | self.start_time = rospy.Time.now() 141 | elapsed = (self.start_time - self.past_time).to_sec() + 1.0e-6 142 | 143 | if elapsed < 1.0 / self.rate * 0.8: 144 | return 145 | 146 | self.past_time = self.start_time 147 | 148 | # calculate odometry 149 | if self.enc_fleft == None: 150 | self.d_fleft = 0 151 | self.d_fright = 0 152 | self.d_rleft = 0 153 | self.d_rright = 0 154 | else: 155 | self.d_fleft = (self.fleft - self.enc_fleft) / self.ticks_meter 156 | self.d_fright = (self.fright - self.enc_fright) / self.ticks_meter 157 | self.d_rleft = (self.rleft - self.enc_rleft) / self.ticks_meter 158 | self.d_rright = (self.rright - self.enc_rright) / self.ticks_meter 159 | 160 | self.enc_fleft = self.fleft 161 | self.enc_fright = self.fright 162 | self.enc_rleft = self.rleft 163 | self.enc_rright = self.rright 164 | 165 | # distance traveled is the average of the 4 wheels 166 | d = ( self.d_fleft + self.d_fright + self.d_rleft + self.d_rright) / 4.0 167 | # this approximation works (in radians) for small angles 168 | th = ( (self.d_fright + self.d_rright)/2.0 - (self.d_fleft + self.d_rleft)/2.0 ) / self.base_width 169 | # calculate velocities 170 | dx_old = self.dx 171 | self.dx = d / elapsed 172 | self.dr = th / elapsed 173 | 174 | if (d != 0): 175 | # calculate distance traveled in x and y 176 | x = cos( th ) * d 177 | y = -sin( th ) * d 178 | # calculate the final position of the robot 179 | self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y ) 180 | self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y ) 181 | if( th != 0): 182 | self.th = self.th + th 183 | 184 | # publish the odom information 185 | quaternion = Quaternion() 186 | quaternion.x = 0.0 187 | quaternion.y = 0.0 188 | quaternion.z = sin( self.th / 2 ) 189 | quaternion.w = cos( self.th / 2 ) 190 | # self.odomBroadcaster.sendTransform( 191 | # (self.x, self.y, 0), 192 | # (quaternion.x, quaternion.y, quaternion.z, quaternion.w), 193 | # rospy.Time.now(), 194 | # self.base_frame_id, 195 | # self.odom_frame_id 196 | # ) 197 | 198 | odom = Odometry() 199 | odom.header.stamp = rospy.Time.now() 200 | odom.header.frame_id = self.odom_frame_id 201 | odom.pose.pose.position.x = self.x 202 | odom.pose.pose.position.y = self.y 203 | odom.pose.pose.position.z = 0 204 | odom.pose.pose.orientation = quaternion 205 | odom.child_frame_id = self.base_frame_id 206 | odom.twist.twist.linear.x = self.dx 207 | odom.twist.twist.linear.y = 0 208 | odom.twist.twist.angular.z = self.dr 209 | self.odomPub.publish(odom) 210 | 211 | 212 | 213 | 214 | ############################################################################# 215 | def flwheelCallback(self, msg): 216 | ############################################################################# 217 | enc = msg.data 218 | if (enc < self.encoder_low_wrap and self.prev_flencoder > self.encoder_high_wrap): 219 | self.flmult = self.flmult + 1 220 | 221 | if (enc > self.encoder_high_wrap and self.prev_flencoder < self.encoder_low_wrap): 222 | self.flmult = self.flmult - 1 223 | 224 | self.fleft = 1.0 * (enc + self.flmult * (self.encoder_max - self.encoder_min)) 225 | self.prev_flencoder = enc 226 | 227 | ############################################################################# 228 | def frwheelCallback(self, msg): 229 | ############################################################################# 230 | enc = msg.data 231 | if(enc < self.encoder_low_wrap and self.prev_frencoder > self.encoder_high_wrap): 232 | self.frmult = self.frmult + 1 233 | 234 | if(enc > self.encoder_high_wrap and self.prev_frencoder < self.encoder_low_wrap): 235 | self.frmult = self.frmult - 1 236 | 237 | self.fright = 1.0 * (enc + self.frmult * (self.encoder_max - self.encoder_min)) 238 | self.prev_frencoder = enc 239 | 240 | ############################################################################# 241 | def rlwheelCallback(self, msg): 242 | ############################################################################# 243 | enc = msg.data 244 | if(enc < self.encoder_low_wrap and self.prev_rlencoder > self.encoder_high_wrap): 245 | self.rlmult = self.rlmult + 1 246 | 247 | if(enc > self.encoder_high_wrap and self.prev_rlencoder < self.encoder_low_wrap): 248 | self.rlmult = self.rlmult - 1 249 | 250 | self.rleft = 1.0 * (enc + self.rlmult * (self.encoder_max - self.encoder_min)) 251 | self.prev_rlencoder = enc 252 | 253 | ############################################################################# 254 | def rrwheelCallback(self, msg): 255 | ############################################################################# 256 | enc = msg.data 257 | if(enc < self.encoder_low_wrap and self.prev_rrencoder > self.encoder_high_wrap): 258 | self.rrmult = self.rrmult + 1 259 | 260 | if(enc > self.encoder_high_wrap and self.prev_rrencoder < self.encoder_low_wrap): 261 | self.rrmult = self.rrmult - 1 262 | 263 | self.rright = 1.0 * (enc + self.rrmult * (self.encoder_max - self.encoder_min)) 264 | self.prev_rrencoder = enc 265 | 266 | ############################################################################# 267 | ############################################################################# 268 | if __name__ == '__main__': 269 | """ main """ 270 | try: 271 | diffTf = DiffTf() 272 | diffTf.spin() 273 | except rospy.ROSInterruptException: 274 | pass 275 | -------------------------------------------------------------------------------- /src/figure_eight.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import numpy as np 6 | import tf.transformations as tr 7 | 8 | from dynamic_reconfigure.server import Server as DynamicReconfigureServer 9 | 10 | # Import custom message data and dynamic reconfigure variables. 11 | from controller_adaptiveclbf.cfg import figureEightConfig as ConfigType 12 | 13 | from nav_msgs.msg import Odometry 14 | 15 | class FigureEightNode(object): 16 | def __init__(self): 17 | """Read in parameters.""" 18 | self.x = 0.0 19 | self.y = 0.0 20 | self.yaw = np.pi/4 21 | 22 | self.start_time = rospy.get_rostime() 23 | self.prev_time = rospy.Time(0) 24 | self.enable = True 25 | 26 | self.params={} 27 | self.params["width"] = rospy.get_param('~width',1.0) 28 | self.params["height"] = rospy.get_param('~height',1.0) 29 | self.params["speed"] = rospy.get_param('~speed',1.0) 30 | self.params["freq"] = rospy.get_param('~freq',0.1) 31 | self.params["x_offset"] = rospy.get_param('~x_offset',0.0) 32 | self.params["y_offset"] = rospy.get_param('~y_offset',0.0) 33 | 34 | # Create a dynamic reconfigure server. 35 | self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) 36 | 37 | # Create publishers 38 | self.pub_target = rospy.Publisher('target', Odometry, queue_size = 1) 39 | 40 | # Initialize message variables. 41 | self.enable = rospy.get_param('~enable', True) 42 | 43 | def timer_cb(self): 44 | if not self.enable: 45 | return 46 | 47 | curr_time = rospy.Time.now() 48 | 49 | t = (curr_time - self.start_time).to_sec() 50 | 51 | # for figure eight 52 | arclength = self.params["width"] * 6 53 | omega = t * 2 * np.pi * self.params["speed"] / arclength 54 | # x_new = self.params["height"] * np.sin(omega) 55 | y_new = self.params["width"] * np.sin(omega) #* np.cos(omega) 56 | x_new = self.params["height"] * np.cos(omega) 57 | 58 | # # for sinusoid 59 | # y_new = self.params["speed"] * t 60 | # x_new = self.params["width"] * np.sin(2 * np.pi * t * self.params["freq"]) 61 | 62 | # Lemniscate of bernoulli (fig 8) 63 | # x_new = self.params["width"] * np.sqrt(2.0) * np.cos(omega) / (np.sin(omega)**2+1) 64 | # y_new = self.params["width"] * np.sqrt(2.0) * np.cos(omega) * np.sin(omega) / (np.sin(omega)**2+1) 65 | 66 | yaw_new = np.arctan2(y_new - self.y, x_new - self.x) 67 | if self.prev_time == rospy.Time(0): 68 | self.vel = 0 69 | else: 70 | self.vel = np.sqrt((self.x - x_new)**2 + (self.y - y_new)**2) / ((curr_time - self.prev_time).to_sec() + 1e-6) 71 | self.x = x_new 72 | self.y = y_new 73 | self.yaw = yaw_new 74 | self.prev_time = curr_time 75 | 76 | # publish reference pose for visualization 77 | target_msg = Odometry() 78 | target_msg.header.frame_id = rospy.get_namespace() + "odom" 79 | target_msg.header.stamp = rospy.get_rostime() 80 | target_msg.pose.pose.position.x = self.x + self.params["x_offset"] 81 | target_msg.pose.pose.position.y = self.y + self.params["y_offset"] 82 | target_msg.pose.pose.position.z = 0.0 83 | target_msg.pose.pose.orientation.x = 0.0 84 | target_msg.pose.pose.orientation.y = 0.0 85 | target_msg.pose.pose.orientation.z = np.sin(self.yaw/2.0) 86 | target_msg.pose.pose.orientation.w = np.cos(self.yaw/2.0) 87 | target_msg.child_frame_id = 'base_link' 88 | target_msg.twist.twist.linear.x = self.vel 89 | 90 | self.pub_target.publish(target_msg) 91 | 92 | def reconfigure_cb(self, config, dummy): 93 | """Create a callback function for the dynamic reconfigure server.""" 94 | # Fill in local variables with values received from dynamic reconfigure 95 | # clients (typically the GUI). 96 | self.params["width"] = config["width"] 97 | self.params["height"] = config["height"] 98 | self.params["speed"] = config["speed"] 99 | self.params["x_offset"] = config["x_offset"] 100 | self.params["y_offset"] = config["y_offset"] 101 | self.params["freq"] = config["freq"] 102 | 103 | # Check to see if node should be started or stopped. 104 | if self.enable != config["enable"]: 105 | if config["enable"]: 106 | self.start_time = rospy.get_rostime() 107 | self.enable = config["enable"] 108 | 109 | # Return the new variables. 110 | return config 111 | 112 | # Main function. 113 | if __name__ == '__main__': 114 | # Initialize the node and name it. 115 | rospy.init_node('figure_eight') 116 | node = FigureEightNode() 117 | rate = rospy.get_param('~rate', 50.0) 118 | r = rospy.Rate(rate) 119 | while not rospy.is_shutdown(): 120 | node.timer_cb() 121 | r.sleep() 122 | -------------------------------------------------------------------------------- /src/lyapunov.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Lyapunov(): 4 | def __init__(self,dim,epsilon=1.0): 5 | self.dim=dim 6 | self.epsilon = epsilon 7 | 8 | def V(self,x): 9 | # overload 10 | return None 11 | 12 | def dV(self,x): 13 | # overload 14 | return None 15 | 16 | def signed_angle_dist(self,a,b): 17 | d = a - b 18 | if d > np.pi: 19 | d = d - np.pi*2 20 | elif d < -np.pi: 21 | d = d + np.pi*2 22 | 23 | return d 24 | 25 | class LyapunovAckermann(Lyapunov): 26 | def __init__(self,dim=4,w1=1.0,w2=1.0,w3=1.0,epsilon=1e-6): 27 | self.w1=w1 28 | self.w2=w2 29 | self.w3=w3 30 | self.epsilon=epsilon 31 | Lyapunov.__init__(self,dim) 32 | 33 | def convert_to_polar(self,x,x_d): 34 | if x[3,:] < 0: 35 | e = -np.sqrt((x_d[0,:]-x[0,:])**2 + (x_d[1,:]-x[1,:])**2) 36 | phi_t = np.arctan2(x[1,:]-x_d[1,:],x[0,:]-x_d[0,:]) 37 | else: 38 | e = np.sqrt((x_d[0,:]-x[0,:])**2 + (x_d[1,:]-x[1,:])**2) 39 | phi_t = np.arctan2(x_d[1,:]-x[1,:],x_d[0,:]-x[0,:]) 40 | alpha = self.signed_angle_dist(phi_t,x[2,:]) 41 | theta = self.signed_angle_dist(phi_t,x_d[2,:]) 42 | 43 | # if alpha and theta are opposite signs, we have two confliciting headings. 44 | # Flip the sign of one to turn in one direction. 45 | # print('before:',alpha,theta) 46 | if np.sign(alpha) != np.sign(theta): 47 | if np.abs(alpha) >= np.abs(theta): 48 | alpha = alpha - np.sign(alpha) * 2 * np.pi 49 | else: 50 | theta = theta - np.sign(theta) * 2 * np.pi 51 | # print('after:',alpha,theta) 52 | # print(e,x[2,:]/(np.pi)*180,phi_t/(np.pi)*180,x_d[2,:]/(np.pi)*180,alpha/(np.pi)*180,theta/(np.pi)*180) 53 | 54 | return e,phi_t,alpha,theta 55 | 56 | def V(self,x,x_d): 57 | e,phi_t,alpha,theta = self.convert_to_polar(x,x_d) 58 | return 0.5 * (self.w1*alpha**2 + self.w2*theta**2 + self.w3*(x[3,:]-x_d[3,:])**2) 59 | 60 | def dV(self,x,x_d): 61 | e,phi_t,alpha,theta = self.convert_to_polar(x,x_d) 62 | c_tmp = (self.w1*alpha + self.w2*theta) / (e + self.epsilon) 63 | return np.stack((c_tmp*np.sin(phi_t),-c_tmp*np.cos(phi_t),-self.w1*alpha,self.w3*(x[3,:]-x_d[3,:])),axis=0) 64 | 65 | class LyapunovAckermannZ(Lyapunov): 66 | def __init__(self,dim=4,w1=1.0, w2=1.0, w3=1.0, epsilon = 1e-6): 67 | self.w1=w1 68 | self.w2=w2 69 | self.w3=w3 70 | self.epsilon = epsilon 71 | Lyapunov.__init__(self,dim) 72 | 73 | 74 | def convert_to_polar(self,z,z_d): 75 | # if z[3,:] < 0: 76 | # e = -np.sqrt((z_d[0,:]-z[0,:])**2 + (z_d[1,:]-z[1,:])**2) 77 | # phi_t = np.arctan2(z[1,:]-z_d[1,:],z[0,:]-z_d[0,:]) 78 | # else: 79 | e = np.sqrt((z_d[0,:]-z[0,:])**2 + (z_d[1,:]-z[1,:])**2) 80 | phi_t = np.arctan2(z_d[1,:]-z[1,:],z_d[0,:]-z[0,:]) 81 | 82 | phi = np.arctan2(z[3,:],z[2,:]) 83 | phi_d = np.arctan2(z_d[3,:],z_d[2,:]) 84 | 85 | alpha = self.signed_angle_dist(phi_t,phi) 86 | theta = self.signed_angle_dist(phi_t,phi_d) 87 | # print('angles: ',phi,phi_t,phi_d,alpha,theta) 88 | # if alpha and theta are opposite signs, we have two confliciting headings. 89 | # Flip the sign of one to turn in one direction. 90 | # print('before:',alpha,theta) 91 | # if np.sign(alpha) != np.sign(theta): 92 | # if np.abs(alpha) >= np.abs(theta): 93 | # alpha = alpha + np.sign(alpha) * 2 * np.pi 94 | # else: 95 | # theta = theta + np.sign(theta) * 2 * np.pi 96 | # print('after:',alpha,theta) 97 | # print(e,z[2,:]/(np.pi)*180,phi_t/(np.pi)*180,z_d[2,:]/(np.pi)*180,alpha/(np.pi)*180,theta/(np.pi)*180) 98 | 99 | return e,phi_t,alpha,theta 100 | 101 | def V(self,z,z_d): 102 | e,phi_t,alpha,theta = self.convert_to_polar(z,z_d) 103 | v = np.sqrt(z[2,:]**2 + z[3,:]**2) 104 | v_d = np.sqrt(z_d[2,:]**2 + z_d[3,:]**2) 105 | return 0.5 * (self.w1*alpha**2 + self.w2*theta**2 + self.w3*(v - v_d)**2) 106 | 107 | def dV(self,z,z_d): 108 | e,phi_t,alpha,theta = self.convert_to_polar(z,z_d) 109 | c_tmp = (self.w1*alpha + self.w2*theta) / (e + self.epsilon) 110 | v = np.sqrt(z[2,:]**2 + z[3,:]**2) 111 | v_d = np.sqrt(z_d[2,:]**2 + z_d[3,:]**2) 112 | 113 | return np.stack((c_tmp*np.sin(phi_t), 114 | -c_tmp*np.cos(phi_t), 115 | self.w1*alpha*z[3,:]/(v**2) + self.w3*(v-v_d)*z[2,:]/v, 116 | -self.w1*alpha*z[2,:]/(v**2) + self.w3*(v-v_d)*z[3,:]/v),axis=0) 117 | -------------------------------------------------------------------------------- /src/model_gp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from scaledgp import ScaledGP 4 | from scipy import signal 5 | from progress.bar import Bar 6 | import random 7 | 8 | # ALPaCA 9 | from ALPaCA.alpaca import * 10 | from ALPaCA.dataset import * 11 | import time 12 | 13 | class Model(): 14 | def __init__(self,xdim,udim,odim,use_obs): 15 | self.xdim=xdim 16 | self.udim=udim 17 | self.odim=odim 18 | self.use_obs = use_obs 19 | self.verbose = True 20 | 21 | def predict(self,x,u,obs): 22 | # overload 23 | return None 24 | 25 | def train(self): 26 | # overload 27 | return None 28 | 29 | def add_data(self,x_next,u,x,obs,dt): 30 | # overload 31 | return None 32 | 33 | class ModelALPaCA(Model): 34 | def __init__(self,xdim,udim,odim,use_obs=False): 35 | Model.__init__(self,xdim,udim,odim,use_obs) 36 | 37 | # note: use use_obs and observations with caution. model may overfit to this input. 38 | model_xdim=self.xdim + 1 39 | if self.use_obs: 40 | model_xdim += self.odim -1 41 | model_ydim=self.xdim/2 42 | 43 | # TODO: put config someplace better (e.g., yaml file) 44 | self.config = { 45 | 'meta_batch_size': 20, 46 | 'data_horizon': 20, 47 | 'test_horizon': 30, 48 | 'lr': 0.01, 49 | 'x_dim': model_xdim, 50 | 'y_dim': model_ydim, 51 | 'sigma_eps': [0.001, 0.001], 52 | 'nn_layers': [128,128,64], 53 | 'activation': 'tanh' 54 | } 55 | 56 | g = tf.Graph() 57 | sess = tf.Session(config=tf.ConfigProto(log_device_placement=True), graph=g) 58 | 59 | self.m = ALPaCA(self.config,sess,g) 60 | self.m.construct_model() 61 | 62 | self.y = np.zeros((0,model_ydim)) 63 | self.z = np.zeros((0,model_xdim)) 64 | self.N_data = 200 65 | self.N_updates = 200 66 | 67 | self.use_obs = False # TODO: remove this later 68 | self.model_trained = False 69 | 70 | def rotate(self,x,theta): 71 | x_body = np.zeros((2,1)) 72 | x_body[0] = x[0] * np.cos(theta) + x[1] * np.sin(theta) 73 | x_body[1] = -x[0] * np.sin(theta) + x[1] * np.cos(theta) 74 | return x_body 75 | 76 | def make_input(self,x,mu,obs): 77 | # format input vector 78 | v = np.sqrt(x[2:3,:]**2 + x[3:4,:]**2) * x[4:5,:] 79 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 80 | theta = obs[0] 81 | x_body = self.rotate(x[2:-1,:],theta) 82 | mu_body = self.rotate(mu,theta) 83 | if self.use_obs: 84 | z = np.concatenate((v,x_body,mu_body,obs[1:,:])).T 85 | else: 86 | z = np.concatenate((v,x_body,mu_body)).T 87 | 88 | return z 89 | 90 | def predict(self,x,mu,obs): 91 | # format the input and use the model to make a prediction. 92 | z_context = np.reshape(self.z,(1,self.z.shape[0],self.config['x_dim'])) 93 | y_context = np.reshape(self.y,(1,self.y.shape[0],self.config['y_dim'])) 94 | 95 | z = self.make_input(x,mu,obs) 96 | z = np.reshape(z,(1,1,self.config['x_dim'])) 97 | 98 | y, var = self.m.test(z_context,y_context,z) 99 | y = np.squeeze(y, axis=0).T 100 | 101 | theta = obs[0] 102 | y_out = self.rotate(y,-theta) 103 | 104 | var = np.diag(np.squeeze(var)) 105 | var = np.expand_dims(var,axis=0).T 106 | return y_out, var 107 | 108 | def train(self): 109 | # train model. this gets called by the training thread on timer_cb() in adaptive_clbf_node. 110 | if self.z.shape[0] > 0: 111 | z_train = np.reshape(self.z,(1,self.z.shape[0],self.z.shape[1])) 112 | y_train = np.reshape(self.y,(1,self.y.shape[0],self.y.shape[1])) 113 | 114 | train_dataset = PresampledDataset(z_train,y_train) 115 | start_time = time.time() 116 | self.m.train(train_dataset, self.N_updates) 117 | if self.verbose: 118 | print('trained, elapsed time: ', time.time() - start_time) 119 | self.model_trained = True 120 | 121 | def add_data(self,x_next,x,mu_model,obs,dt): 122 | # add a sample to the history of data 123 | x_dot = (x_next[2:-1,:]-x[2:-1,:])/dt 124 | ynew = x_dot - mu_model 125 | znew = self.make_input(x,x_dot,obs) 126 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 127 | theta=obs[0] 128 | ynew_rotated = self.rotate(ynew,theta) 129 | self.y = np.concatenate((self.y,ynew_rotated.T)) 130 | self.z = np.concatenate((self.z,znew)) 131 | 132 | # throw away old samples if too many samples collected. 133 | if self.y.shape[0] > self.N_data: 134 | self.y = self.y[-self.N_data:,:] 135 | self.z = self.z[-self.N_data:,:] 136 | # self.y = np.delete(self.y,random.randint(0,self.N_data-1),axis=0) 137 | # self.z = np.delete(self.z,random.randint(0,self.N_data-1),axis=0) 138 | 139 | if self.verbose: 140 | print("obs", obs) 141 | print("y_out",y_out) 142 | print("ynew",ynew) 143 | print("znew",znew) 144 | print("x_dot",x_dot) 145 | print("mu_model",mu_model) 146 | print("dt",dt) 147 | print("n data:", self.y.shape[0]) 148 | print("prediction error:", self.predict_error) 149 | print("predict var:", self.predict_var) 150 | 151 | class ModelGP(Model): 152 | def __init__(self,xdim,udim,odim,use_obs=False): 153 | Model.__init__(self,xdim,udim,odim,use_obs) 154 | # note: use use_obs and observations with caution. model may overfit to this input. 155 | model_xdim=self.xdim + 1 156 | if self.use_obs: 157 | model_xdim += self.odim -1 158 | model_ydim=self.xdim/2 159 | 160 | self.m = ScaledGP(xdim=model_xdim,ydim=model_ydim) 161 | self.y = np.zeros((0,model_ydim)) 162 | self.z = np.zeros((0,model_xdim)) 163 | self.N_data = 400 164 | 165 | self.model_trained = False 166 | 167 | def rotate(self,x,theta): 168 | x_body = np.zeros((2,1)) 169 | x_body[0] = x[0] * np.cos(theta) + x[1] * np.sin(theta) 170 | x_body[1] = -x[0] * np.sin(theta) + x[1] * np.cos(theta) 171 | return x_body 172 | 173 | def make_input(self,x,mu,obs): 174 | # format input vector 175 | v = np.sqrt(x[2:3,:]**2 + x[3:4,:]**2) * x[4:5,:] 176 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 177 | theta = obs[0] 178 | x_body = self.rotate(x[2:-1,:],theta) 179 | mu_body = self.rotate(mu,theta) 180 | if self.use_obs: 181 | z = np.concatenate((v,x_body,mu_body,obs[1:,:])).T 182 | else: 183 | z = np.concatenate((v,x_body,mu_body)).T 184 | 185 | return z 186 | 187 | def predict(self,x,mu,obs): 188 | # format the input and use the model to make a prediction. 189 | z = self.make_input(x,mu,obs) 190 | y, var = self.m.predict(z) 191 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 192 | theta=obs[0] 193 | y_out = self.rotate(y.T,-theta) 194 | return y_out, var.T 195 | 196 | def train(self): 197 | # train model. this gets called by the training thread on timer_cb() in adaptive_clbf_node. 198 | if self.z.shape[0] > 0: 199 | self.m.optimize(self.z,self.y) 200 | self.model_trained = True 201 | 202 | def add_data(self,x_next,x,mu_model,obs,dt): 203 | # add a sample to the history of data 204 | x_dot = (x_next[2:-1,:]-x[2:-1,:])/dt 205 | ynew = x_dot - mu_model 206 | znew = self.make_input(x,x_dot,obs) 207 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 208 | theta=obs[0] 209 | ynew_rotated = self.rotate(ynew,theta) 210 | self.y = np.concatenate((self.y,ynew_rotated.T)) 211 | self.z = np.concatenate((self.z,znew)) 212 | 213 | # throw away old samples if too many samples collected. 214 | if self.y.shape[0] > self.N_data: 215 | self.y = self.y[-self.N_data:,:] 216 | self.z = self.z[-self.N_data:,:] 217 | # self.y = np.delete(self.y,random.randint(0,self.N_data-1),axis=0) 218 | # self.z = np.delete(self.z,random.randint(0,self.N_data-1),axis=0) 219 | 220 | if self.verbose: 221 | print("obs", obs) 222 | print("y_out",y_out) 223 | print("ynew",ynew) 224 | print("znew",znew) 225 | print("x_dot",x_dot) 226 | print("mu_model",mu_model) 227 | print("dt",dt) 228 | print("n data:", self.y.shape[0]) 229 | print("prediction error:", self.predict_error) 230 | print("predict var:", self.predict_var) 231 | -------------------------------------------------------------------------------- /src/model_service.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import os 3 | 4 | # don't use gpu 5 | os.environ['CUDA_VISIBLE_DEVICES'] = '-1' 6 | 7 | import rospy 8 | import actionlib 9 | import numpy as np 10 | 11 | from dynamic_reconfigure.client import Client as DynamicReconfigureClient 12 | 13 | import controller_adaptiveclbf.msg 14 | from controller_adaptiveclbf.srv import * 15 | 16 | import numpy as np 17 | import matplotlib.pyplot as plt 18 | from scaledgp import ScaledGP 19 | from scipy import signal 20 | from progress.bar import Bar 21 | import random 22 | 23 | # ALPaCA 24 | from ALPaCA.alpaca import * 25 | from ALPaCA.dataset import * 26 | import time 27 | 28 | # Vannila Netowrk 29 | from vanilla_nn import VanillaNN 30 | 31 | BASE_PATH = os.path.expanduser('~/Documents') 32 | 33 | class ModelService(object): 34 | _train_result = controller_adaptiveclbf.msg.TrainModelResult() 35 | 36 | def __init__(self,xdim,odim,use_obs, use_service = True): 37 | 38 | self.xdim=xdim 39 | self.odim=odim 40 | self.use_obs = use_obs 41 | self.verbose = True 42 | 43 | self.config = {} 44 | 45 | if use_service: 46 | # train action server 47 | self._action_service = actionlib.SimpleActionServer('train_model_service', controller_adaptiveclbf.msg.TrainModelAction, execute_cb=self.train, auto_start = False) 48 | self._action_service.start() 49 | # add data service 50 | self._add_data_srv = rospy.Service('add_data_2_model', AddData2Model, self.add_data) 51 | # predict service 52 | self._predict_srv = rospy.Service('predict_model', PredictModel, self.predict) 53 | 54 | # Create a dynamic reconfigure client 55 | self.dyn_reconfig_client = DynamicReconfigureClient('controller_adaptiveclbf_reconfig', timeout=30, config_callback=self.reconfigure_cb) 56 | 57 | self.N_data = rospy.get_param('/controller_adaptiveclbf/N_data',200) 58 | self.verbose = rospy.get_param('/controller_adaptiveclbf/learning_verbose',False) 59 | 60 | 61 | def reconfigure_cb(self, config): 62 | self.N_data = config["N_data"] 63 | self.verbose = config["learning_verbose"] 64 | self.N_updates = config["N_updates"] 65 | self.config["meta_batch_size"] = config["meta_batch_size"] 66 | self.config["data_horizon"] = config["data_horizon"] 67 | self.config["test_horizon"] = config["test_horizon"] 68 | self.config["learning_rate"] = config["learning_rate"] 69 | self.config["min_datapoints"] = config["min_datapoints"] 70 | self.config["save_data_interval"] = config["save_data_interval"] 71 | 72 | def predict(self,req): 73 | # overload 74 | return None 75 | 76 | def train(self,goal): 77 | # overload 78 | return None 79 | 80 | def add_data(self,req): 81 | # overload 82 | return None 83 | 84 | 85 | def scale_data(self,x,xmean,xstd): 86 | if (xstd == 0).any(): 87 | return (x-xmean) 88 | else: 89 | return (x - xmean) / xstd 90 | 91 | def unscale_data(self,x,xmean,xstd): 92 | if (xstd == 0).any(): 93 | return x + xmean 94 | else: 95 | return x * xstd + xmean 96 | 97 | class ModelVanillaService(ModelService): 98 | def __init__(self,xdim,odim,use_obs=False,use_service=True): 99 | ModelService.__init__(self,xdim,odim,use_obs,use_service) 100 | # note: use use_obs and observations with caution. model may overfit to this input. 101 | model_xdim=self.xdim/2 102 | if self.use_obs: 103 | model_xdim += self.odim 104 | model_ydim=self.xdim/2 105 | 106 | self.Zmean = np.zeros((1,model_xdim)) 107 | self.Zstd = np.ones((1,model_xdim)) 108 | self.ymean = np.zeros((1,model_ydim)) 109 | self.ystd = np.ones((1,model_ydim)) 110 | self.model_trained = False 111 | 112 | self.y = np.zeros((0,model_ydim)) 113 | self.Z = np.zeros((0,model_xdim)) 114 | 115 | self.m = VanillaNN(model_xdim, model_ydim) 116 | 117 | 118 | def rotate(self,x,theta): 119 | x_body = np.zeros((2,1)) 120 | x_body[0] = x[0] * np.cos(theta) + x[1] * np.sin(theta) 121 | x_body[1] = -x[0] * np.sin(theta) + x[1] * np.cos(theta) 122 | return x_body 123 | 124 | def make_input(self,x,obs): 125 | # format input vector 126 | theta = obs[0] 127 | x_body = self.rotate(x[2:-1,:],theta) 128 | if self.use_obs: 129 | Z = np.concatenate((x_body,obs[1:,:])).T 130 | else: 131 | Z = np.concatenate((x_body)).T 132 | 133 | Z = self.scale_data(Z,self.Zmean,self.Zstd) 134 | return Z 135 | 136 | def predict(self,req): 137 | if not hasattr(self, 'Z'): 138 | resp = PredictModelResponse() 139 | resp.result = False 140 | return resp 141 | x = np.expand_dims(req.x, axis=0).T 142 | obs = np.expand_dims(req.obs, axis=0).T 143 | 144 | Z = self.make_input(x,obs) 145 | 146 | # needed fix for weird shapes of tensors 147 | Y = self.y[-1:,:] 148 | y, var, _ = self.m.predict(Z,Y) 149 | theta = obs[0] 150 | y_out = self.rotate(y,-theta).T 151 | 152 | resp = PredictModelResponse() 153 | resp.y_out = y_out.flatten() 154 | resp.var = var 155 | resp.result = True 156 | 157 | return resp 158 | 159 | def train(self, goal=None): 160 | success = True 161 | 162 | if goal is not None: 163 | # goal was cancelled 164 | if self._action_service.is_preempt_requested(): 165 | print("Preempt training request") 166 | self._action_service.set_preempted() 167 | success = False 168 | 169 | # train model. this gets called by the training thread on timer_cb() in adaptive_clbf_node. 170 | # print("train z then y shapes: ", self.Z.shape, self.y.shape) 171 | if success and self.Z.shape[0] > 0 and self.Z.shape[0] == self.y.shape[0]: 172 | self.m.train(self.Z, self.y,_epoch=self.N_updates,_batch_size=self.config["meta_batch_size"]) 173 | if goal is not None: 174 | self._train_result.model_trained = True 175 | self._action_service.set_succeeded(self._train_result) 176 | else: 177 | if goal is not None: 178 | self._train_result.model_trained = False 179 | self._action_service.set_succeeded(self._train_result) 180 | 181 | def add_data(self,req): 182 | if not hasattr(self, 'y'): 183 | return AddData2ModelResponse(False) 184 | 185 | x_next = np.expand_dims(req.x_next, axis=0).T 186 | x = np.expand_dims(req.x, axis=0).T 187 | mu_model = np.expand_dims(req.mu_model, axis=0).T 188 | obs = np.expand_dims(req.obs, axis=0).T 189 | dt = req.dt 190 | 191 | # add a sample to the history of data 192 | x_dot = (x_next[2:-1,:]-x[2:-1,:])/dt 193 | ynew = x_dot - mu_model 194 | Znew = self.make_input(x,obs) 195 | 196 | theta=obs[0] 197 | ynew_rotated = self.rotate(ynew,theta) 198 | self.y = np.concatenate((self.y,ynew_rotated.T)) 199 | self.Z = np.concatenate((self.Z,Znew)) 200 | 201 | # throw away old samples if too many samples collected. 202 | if self.y.shape[0] > self.N_data: 203 | self.y = self.y[-self.N_data:,:] 204 | self.Z = self.Z[-self.N_data:,:] 205 | # self.y = np.delete(self.y,random.randint(0,self.N_data-1),axis=0) 206 | # self.Z = np.delete(self.Z,random.randint(0,self.N_data-1),axis=0) 207 | 208 | if self.verbose: 209 | print("obs", obs) 210 | print("ynew",ynew) 211 | print("ynew_rotated", ynew_rotated) 212 | print("Znew",Znew) 213 | print("x_dot",x_dot) 214 | print("mu_model",mu_model) 215 | print("dt",dt) 216 | print("n data:", self.y.shape[0]) 217 | 218 | return AddData2ModelResponse(True) 219 | 220 | class ModelALPaCAService(ModelService): 221 | def __init__(self,xdim,odim,use_obs=False,use_service=True): 222 | ModelService.__init__(self,xdim,odim,use_obs,use_service) 223 | # note: use use_obs and observations with caution. model may overfit to this input. 224 | model_xdim=self.xdim/2 225 | if self.use_obs: 226 | model_xdim += self.odim 227 | model_ydim=self.xdim/2 228 | 229 | self.Zmean = np.zeros((1,model_xdim)) 230 | self.Zstd = np.ones((1,model_xdim)) 231 | self.ymean = np.zeros((1,model_ydim)) 232 | self.ystd = np.ones((1,model_ydim)) 233 | self.model_trained = False 234 | 235 | # TODO: put config someplace better (e.g., yaml file) 236 | self.config = { 237 | 'meta_batch_size': 150, 238 | 'data_horizon': 20, 239 | 'test_horizon': 20, 240 | 'lr': 0.01, 241 | 'x_dim': model_xdim, 242 | 'y_dim': model_ydim, 243 | 'sigma_eps': [1.0, 1.0], 244 | 'nn_layers': [128,128,128], 245 | 'activation': 'sigmoid', 246 | # 'min_datapoints': 1000, 247 | 'min_datapoints': 500, 248 | 'save_data_interval': 1000, 249 | # 'sigma_eps': [1e-2, 1e-2] 250 | } 251 | 252 | self.use_service = use_service 253 | # for test_adaptive_clbf.py 254 | if not self.use_service: 255 | self.config['sigma_eps'] = [1e-2,1e-2] 256 | 257 | g = tf.Graph() 258 | config = tf.ConfigProto(log_device_placement=True) 259 | config.gpu_options.allow_growth=True 260 | sess = tf.Session(config=config, graph=g) 261 | 262 | self.m = ALPaCA(self.config,sess,g) 263 | self.m.construct_model() 264 | 265 | self.y = np.zeros((0,model_ydim)) 266 | self.Z = np.zeros((0,model_xdim)) 267 | self.N_data = 10000 268 | self.N_updates = 50 269 | 270 | def rotate(self,x,theta): 271 | x_body = np.zeros((2,1)) 272 | x_body[0] = x[0] * np.cos(theta) + x[1] * np.sin(theta) 273 | x_body[1] = -x[0] * np.sin(theta) + x[1] * np.cos(theta) 274 | return x_body 275 | 276 | def make_input(self,x,obs): 277 | # format input vector 278 | theta = obs[0] 279 | x_body = self.rotate(x[2:-1,:],theta) 280 | if self.use_obs: 281 | Z = np.concatenate((x_body,obs[1:,:])).T 282 | else: 283 | Z = np.concatenate((x_body)).T 284 | 285 | if self.use_service: 286 | Z = self.scale_data(Z,self.Zmean,self.Zstd) 287 | return Z 288 | 289 | def predict(self,req): 290 | if not hasattr(self, 'Z'): 291 | resp = PredictModelResponse() 292 | resp.result = False 293 | return resp 294 | x = np.expand_dims(req.x, axis=0).T 295 | obs = np.expand_dims(req.obs, axis=0).T 296 | 297 | # format the input and use the model to make a prediction. 298 | z_context = np.reshape(self.Z[-self.config["data_horizon"]:,:],(1,np.minimum(self.config["data_horizon"],self.Z.shape[0]),self.config['x_dim'])) 299 | y_context = np.reshape(self.y[-self.config["data_horizon"]:,:],(1,np.minimum(self.config["data_horizon"],self.y.shape[0]),self.config['y_dim'])) 300 | 301 | Z = self.make_input(x,obs) 302 | Z = np.reshape(Z,(1,1,self.config['x_dim'])) 303 | 304 | y, var = self.m.test(z_context,y_context,Z) 305 | y = np.squeeze(y, axis=0).T 306 | 307 | theta = obs[0] 308 | y_out = self.rotate(y,-theta).T 309 | # y_out = self.unscale_data(y_out,self.ymean,self.ystd) 310 | 311 | var = np.diag(np.squeeze(var)) 312 | # var = np.expand_dims(var,axis=0).T 313 | resp = PredictModelResponse() 314 | resp.y_out = y_out.flatten() 315 | resp.var = var - self.config["sigma_eps"] 316 | resp.result = True 317 | return resp 318 | 319 | def train(self,goal=None): 320 | success = True 321 | 322 | if goal is not None: 323 | # goal was cancelled 324 | if self._action_service.is_preempt_requested(): 325 | print("Preempt training request") 326 | self._action_service.set_preempted() 327 | success = False 328 | 329 | if not hasattr(self, 'Z'): 330 | success = False 331 | 332 | # train model. this gets called by the training thread on timer_cb() in adaptive_clbf_node. 333 | if success and self.Z.shape[0] > self.config["min_datapoints"] and self.Z.shape[0] == self.y.shape[0]: 334 | if not self.model_trained: 335 | self.Zmean = np.mean(self.Z,axis=0) 336 | self.Zstd = np.std(self.Z,axis=0) 337 | 338 | if self.use_service: 339 | self.Z = self.scale_data(self.Z,self.Zmean, self.Zstd) 340 | 341 | self.ymean = np.mean(self.y,axis=0) 342 | self.ystd = np.std(self.y,axis=0) 343 | 344 | if self.use_service: 345 | self.y = self.scale_data(self.y,self.ymean, self.ystd) 346 | 347 | self.model_trained = True 348 | print("Mean and std of data computed with # data points:", self.Z.shape[0]) 349 | print("Zmean:", self.Zmean) 350 | print("Zstd:", self.Zstd) 351 | print("ymean:", self.ymean) 352 | print("ystd:", self.ystd) 353 | 354 | 355 | z_train = np.reshape(self.Z,(1,self.Z.shape[0],self.Z.shape[1])) 356 | y_train = np.reshape(self.y,(1,self.y.shape[0],self.y.shape[1])) 357 | train_dataset = PresampledDataset(z_train,y_train) 358 | # start_time = time.time() 359 | self.m.train(train_dataset, self.N_updates, self.config) 360 | # print('trained, elapsed time: ', time.time() - start_time) 361 | if goal is not None: 362 | self._train_result.model_trained = True 363 | self._action_service.set_succeeded(self._train_result) 364 | else: 365 | if goal is not None: 366 | self._train_result.model_trained = False 367 | self._action_service.set_succeeded(self._train_result) 368 | 369 | def add_data(self,req): 370 | if not hasattr(self, 'y'): 371 | return AddData2ModelResponse(False) 372 | 373 | x_next = np.expand_dims(req.x_next, axis=0).T 374 | x = np.expand_dims(req.x, axis=0).T 375 | mu_model = np.expand_dims(req.mu_model, axis=0).T 376 | obs = np.expand_dims(req.obs, axis=0).T 377 | dt = req.dt 378 | 379 | # add a sample to the history of data 380 | x_dot = (x_next[2:-1,:]-x[2:-1,:])/dt 381 | ynew = x_dot - mu_model 382 | Znew = self.make_input(x,obs) 383 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 384 | theta=obs[0] 385 | ynew_rotated = self.rotate(ynew,theta).T 386 | # ynew_rotated = self.scale_data(ynew_rotated,self.ymean,self.ystd) 387 | self.y = np.concatenate((self.y,ynew_rotated)) 388 | self.Z = np.concatenate((self.Z,Znew)) 389 | 390 | # throw away old samples if too many samples collected. 391 | if self.y.shape[0] > self.N_data: 392 | self.y = self.y[-self.N_data:,:] 393 | self.Z = self.Z[-self.N_data:,:] 394 | # self.y = np.delete(self.y,random.randint(0,self.N_data-1),axis=0) 395 | # self.Z = np.delete(self.Z,random.randint(0,self.N_data-1),axis=0) 396 | 397 | # if self.y.shape[0] % self.config["save_data_interval"] == 1: 398 | # print("Saving data to " + BASE_PATH + "...") 399 | # np.save(os.path.join(BASE_PATH, "data_input"),self.Z) 400 | # np.save(os.path.join(BASE_PATH, "data_output"),self.y) 401 | # print("Data saved!") 402 | 403 | if self.verbose: 404 | print("obs", obs) 405 | # print("y_out",y_out) 406 | print("ynew",ynew) 407 | print("Znew",Znew) 408 | print("x_dot",x_dot) 409 | print("mu_model",mu_model) 410 | print("dt",dt) 411 | print("n data:", self.y.shape[0]) 412 | # print("prediction error:", self.predict_error) 413 | # print("predict var:", self.predict_var) 414 | 415 | return AddData2ModelResponse(True) 416 | 417 | class ModelGPService(ModelService): 418 | def __init__(self,xdim,odim,use_obs=False,use_service=True): 419 | ModelService.__init__(self,xdim,odim,use_obs,use_service) 420 | # note: use use_obs and observations with caution. model may overfit to this input. 421 | model_xdim=self.xdim/2 422 | if self.use_obs: 423 | model_xdim += self.odim 424 | model_ydim=self.xdim/2 425 | 426 | self.m = ScaledGP(xdim=model_xdim,ydim=model_ydim) 427 | self.y = np.zeros((0,model_ydim)) 428 | self.Z = np.zeros((0,model_xdim)) 429 | self.N_data = 400 430 | 431 | def rotate(self,x,theta): 432 | x_body = np.zeros((2,1)) 433 | x_body[0] = x[0] * np.cos(theta) + x[1] * np.sin(theta) 434 | x_body[1] = -x[0] * np.sin(theta) + x[1] * np.cos(theta) 435 | return x_body 436 | 437 | def make_input(self,x,obs): 438 | # format input vector 439 | theta = obs[0] 440 | x_body = self.rotate(x[2:-1,:],theta) 441 | if self.use_obs: 442 | Z = np.concatenate((x_body,obs[1:,:])).T 443 | else: 444 | Z = np.concatenate((x_body)).T 445 | 446 | #normalize input by mean and variance 447 | # Z = (Z - self.Zmean) / self.Zvar 448 | 449 | return Z 450 | 451 | def predict(self,req): 452 | if not hasattr(self, 'm'): 453 | resp = PredictModelResponse() 454 | resp.result = False 455 | return resp 456 | x = np.expand_dims(req.x, axis=0).T 457 | obs = np.expand_dims(req.obs, axis=0).T 458 | 459 | # format the input and use the model to make a prediction. 460 | Z = self.make_input(x,obs) 461 | y, var = self.m.predict(Z) 462 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 463 | theta=obs[0] 464 | y_out = self.rotate(y.T,-theta) 465 | 466 | resp = PredictModelResponse() 467 | resp.y_out = y_out.flatten() 468 | resp.var = var.T.flatten() 469 | resp.result = True 470 | 471 | return resp 472 | 473 | def train(self, goal=None): 474 | success = True 475 | 476 | if goal is not None: 477 | # goal was cancelled 478 | if self._action_service.is_preempt_requested(): 479 | print("Preempt training request") 480 | self._action_service.set_preempted() 481 | success = False 482 | 483 | # train model. this gets called by the training thread on timer_cb() in adaptive_clbf_node. 484 | if success and self.Z.shape[0] > 0 and self.Z.shape[0] == self.y.shape[0]: 485 | self.m.optimize(self.Z,self.y) 486 | if goal is not None: 487 | self._train_result.model_trained = True 488 | self._action_service.set_succeeded(self._train_result) 489 | else: 490 | if goal is not None: 491 | self._train_result.model_trained = False 492 | self._action_service.set_succeeded(self._train_result) 493 | def add_data(self,req): 494 | if not hasattr(self, 'y'): 495 | return AddData2ModelResponse(False) 496 | 497 | x_next = np.expand_dims(req.x_next, axis=0).T 498 | x = np.expand_dims(req.x, axis=0).T 499 | mu_model = np.expand_dims(req.mu_model, axis=0).T 500 | obs = np.expand_dims(req.obs, axis=0).T 501 | dt = req.dt 502 | 503 | # add a sample to the history of data 504 | x_dot = (x_next[2:-1,:]-x[2:-1,:])/dt 505 | ynew = x_dot - mu_model 506 | Znew = self.make_input(x,obs) 507 | # theta = np.arctan2(x[3]*x[4],x[2]*x[4]) 508 | theta=obs[0] 509 | ynew_rotated = self.rotate(ynew,theta) 510 | self.y = np.concatenate((self.y,ynew_rotated.T)) 511 | self.Z = np.concatenate((self.Z,Znew)) 512 | 513 | # throw away old samples if too many samples collected. 514 | if self.y.shape[0] > self.N_data: 515 | self.y = self.y[-self.N_data:,:] 516 | self.Z = self.Z[-self.N_data:,:] 517 | # self.y = np.delete(self.y,random.randint(0,self.N_data-1),axis=0) 518 | # self.Z = np.delete(self.Z,random.randint(0,self.N_data-1),axis=0) 519 | 520 | if self.verbose: 521 | print("obs", obs) 522 | print("ynew",ynew) 523 | print("ynew_rotated", ynew_rotated) 524 | print("Znew",Znew) 525 | print("x_dot",x_dot) 526 | print("mu_model",mu_model) 527 | print("dt",dt) 528 | print("n data:", self.y.shape[0]) 529 | # print("prediction error:", self.predict_error) 530 | # print("predict var:", self.predict_var) 531 | 532 | return AddData2ModelResponse(True) 533 | 534 | if __name__ == '__main__': 535 | rospy.init_node('model_service') 536 | server = ModelVanillaService(4,6, use_obs = True) # TODO: put this in yaml or somewhere else 537 | # server = ModelALPaCAService(4,6, use_obs = True) # TODO: put this in yaml or somewhere else 538 | # server = ModelGPService(4,6, use_obs = True) # TODO: put this in yaml or somewhere else 539 | rospy.spin() 540 | -------------------------------------------------------------------------------- /src/plot_bagged_data.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import numpy as np 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | matplotlib.rcParams['pdf.fonttype'] = 42 6 | matplotlib.rcParams['ps.fonttype'] = 42 7 | 8 | BASE_PATH = '/home/nereid/learn2adapt_ws/' 9 | 10 | # retrieves data from rosbag & stores them in arrays 11 | def get_data(bag): 12 | odom = np.array([[0,0]]) 13 | ref = np.array([[0,0]]) 14 | time_elapsed = np.array([0]) 15 | 16 | for topic, msg, t in bag.read_messages(topics=['/downward/vio/odometry', '/reference_vis','/time_elapsed']): 17 | 18 | if topic == '/downward/vio/odometry': 19 | odom = np.append(odom,[[msg.pose.pose.position.x, msg.pose.pose.position.y]],axis=0) 20 | 21 | if topic == '/reference_vis': 22 | ref = np.append(ref,[[msg.pose.pose.position.x, msg.pose.pose.position.y]],axis=0) 23 | 24 | if topic == '/time_elapsed': 25 | time_elapsed = np.append(time_elapsed, [msg.data.to_sec()], axis=0) 26 | 27 | return odom, ref, time_elapsed 28 | 29 | def error(x1,x2): 30 | return np.sqrt((x1[:,0] - x2[:,0])**2 + (x1[:,1] - x2[:,1])**2) 31 | 32 | with rosbag.Bag(BASE_PATH + 'nolearning3.bag', 'r') as bag: 33 | nolearn_odom, nolearn_ref, nolearn_time = get_data(bag) 34 | nolearn_err = error(nolearn_odom, nolearn_ref) 35 | bag.close() 36 | 37 | with rosbag.Bag(BASE_PATH + 'test.bag', 'r') as bag: 38 | alpaca_odom, alpaca_ref, alpaca_time = get_data(bag) 39 | alpaca_err = error(alpaca_odom, alpaca_ref) 40 | bag.close() 41 | 42 | with rosbag.Bag(BASE_PATH + 'vanilla3.bag', 'r') as bag: 43 | vanilla_odom, vanilla_ref, vanilla_time = get_data(bag) 44 | vanilla_err = error(vanilla_odom, vanilla_ref) 45 | bag.close() 46 | 47 | with rosbag.Bag(BASE_PATH + 'gp3.bag', 'r') as bag: 48 | gp_odom, gp_ref, gp_time = get_data(bag) 49 | gp_err = error(gp_odom, gp_ref) 50 | bag.close() 51 | 52 | 53 | # plots combined pose errors 54 | plt.figure() 55 | plt.rcParams.update({'font.size': 12}) 56 | plt.plot(nolearn_time, nolearn_err) 57 | plt.plot(alpaca_time, alpaca_err) 58 | plt.plot(vanilla_time, vanilla_err) 59 | plt.plot(gp_time, gp_err) 60 | # line for when model was used 61 | x1, y1 = [[30, 30], [0, np.max(alpaca_err)]] 62 | plt.plot(x1,y1) 63 | plt.legend(['nolearn', 'alpaca', 'vanilla', 'gp']) 64 | plt.ylabel("Pose error (m)") 65 | plt.xlabel("Time (s)") 66 | 67 | # -- subplots of last 30s of pose errors 68 | plt.figure() 69 | plt.rcParams.update({'font.size': 12}) 70 | plt.subplot(411) 71 | plt.plot(nolearn_time[-nolearn_time.shape[0]/4:], nolearn_err[-nolearn_time.shape[0]/4:]) 72 | plt.ylabel("Pose error (m)") 73 | plt.legend(["No Learning"]) 74 | 75 | plt.subplot(412) 76 | plt.plot(alpaca_time[-alpaca_time.shape[0]/4:], alpaca_err[-alpaca_time.shape[0]/4:]) 77 | plt.ylabel("Pose error (m)") 78 | plt.legend(["ALPaCA"]) 79 | 80 | plt.subplot(413) 81 | plt.plot(vanilla_time[-vanilla_time.shape[0]/4:], vanilla_err[-vanilla_time.shape[0]/4:]) 82 | plt.ylabel("Pose error (m)") 83 | plt.legend(["VanillaNN"]) 84 | 85 | plt.subplot(414) 86 | plt.plot(gp_time[-gp_time.shape[0]/4:], gp_err[-gp_time.shape[0]/4:]) 87 | plt.ylabel("Pose error (m)") 88 | plt.xlabel("Time (s)") 89 | plt.legend(["GP"]) 90 | 91 | # -- plots xy position 92 | plt.figure() 93 | plt.subplot(221) 94 | plt.rcParams.update({'font.size': 12}) 95 | plt.plot(nolearn_odom[:,0],nolearn_odom[:,1]) 96 | plt.plot(nolearn_ref[:,0],nolearn_ref[:,1]) 97 | plt.xlabel('x') 98 | plt.ylabel('y') 99 | plt.legend(['odom', 'ref']) 100 | plt.title('No Learning') 101 | 102 | plt.subplot(222) 103 | plt.rcParams.update({'font.size': 12}) 104 | plt.plot(alpaca_odom[:,0],alpaca_odom[:,1]) 105 | plt.plot(alpaca_ref[:,0],alpaca_ref[:,1]) 106 | plt.xlabel('x') 107 | plt.ylabel('y') 108 | plt.legend(['odom', 'ref']) 109 | plt.title('ALPaCA') 110 | 111 | plt.subplot(223) 112 | plt.rcParams.update({'font.size': 12}) 113 | plt.plot(vanilla_odom[:,0],vanilla_odom[:,1]) 114 | plt.plot(vanilla_ref[:,0],vanilla_ref[:,1]) 115 | plt.xlabel('x') 116 | plt.ylabel('y') 117 | plt.legend(['odom', 'ref']) 118 | plt.title('Vanilla') 119 | 120 | plt.subplot(224) 121 | plt.rcParams.update({'font.size': 12}) 122 | plt.plot(gp_odom[:,0],gp_odom[:,1]) 123 | plt.plot(gp_ref[:,0],gp_ref[:,1]) 124 | plt.xlabel('x') 125 | plt.ylabel('y') 126 | plt.legend(['odom', 'ref']) 127 | plt.title('GP') 128 | 129 | plt.show() 130 | -------------------------------------------------------------------------------- /src/qp_solver.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import osqp 3 | import scipy.sparse as sparse 4 | import scipy.linalg as sl 5 | import rospy 6 | 7 | class QPSolve(): 8 | def __init__(self,dyn,clf,cbf_list,u_lim,u_cost=0.0,u_prev_cost=1.0,p1_cost=1.0e10,p2_cost=1.0e10,verbose=True): 9 | self.xdim = dyn.xdim 10 | self.udim = dyn.udim 11 | self.dyn = dyn 12 | self.clf = clf 13 | self.cbf_list = cbf_list 14 | self.u_lim = u_lim 15 | self.p1_cost = p1_cost 16 | self.p2_cost = p2_cost 17 | self.verbose = verbose 18 | self.u_cost = u_cost 19 | self.u_prev_cost = u_prev_cost 20 | self.K = 0.0 21 | self.ksig = 1.0 22 | self.max_var = 1.0 23 | self.mu_qp_prev = np.zeros((self.xdim/2,1), dtype=np.float32) 24 | self.P = np.eye(self.xdim, dtype=np.float32) 25 | self.A = np.zeros((self.xdim,self.xdim), dtype=np.float32) 26 | self.A0 = np.block([[np.zeros((self.xdim/2,self.xdim/2)),np.eye(self.xdim/2)],[np.zeros((self.xdim/2,self.xdim/2)),np.zeros((self.xdim/2,self.xdim/2))]]).astype(np.float32) 27 | self.G = np.block([[np.zeros((self.xdim/2,self.xdim/2))],[np.eye(self.xdim/2)]]).astype(np.float32) 28 | self.res = None 29 | self.max_error = 1.0 30 | 31 | def update_ricatti(self,A): 32 | self.A = A 33 | Q = np.eye(self.xdim, dtype=np.float32) 34 | self.P = sl.solve_continuous_are(self.A,np.zeros((self.xdim,self.xdim), dtype=np.float32),Q,np.eye(self.xdim, dtype=np.float32)) 35 | 36 | def solve(self,x,x_d,mu_d,sigDelta): 37 | sigDelta = sigDelta * self.ksig 38 | sigDelta = np.clip(sigDelta,0.0,self.max_var) 39 | # sigDelta = np.ones((self.xdim/2,1)) * self.max_var # for testing 40 | 41 | # build Q and p matrices to specify minimization expression 42 | Q = np.diag(np.append(np.append(np.ones(self.xdim/2, dtype=np.float32)*(self.u_cost + self.u_prev_cost),self.p1_cost),self.p2_cost)) 43 | self.Q = sparse.csc_matrix(Q) 44 | self.p = 2*np.append(np.append(-self.mu_qp_prev*self.u_prev_cost,0),0) 45 | 46 | #error dynamics for clf 47 | e = x[:-1,:]-x_d[:-1,:] 48 | e = np.clip(e,-self.max_error,self.max_error) 49 | eTPG = np.matmul(e.T,np.matmul(self.P,self.G)) 50 | G_dyn = np.expand_dims(np.append(np.append(eTPG,1),0),0) #append 1 for clf < d 51 | Gsig = np.matmul(self.G,sigDelta) 52 | GssG = np.matmul(Gsig,Gsig.T) 53 | self.trGssGP = np.trace(np.matmul(GssG,self.P)) 54 | h_dyn = -1 * ( -0.5*np.matmul(e.T,np.matmul(Q,e)) 55 | + 0.5*np.matmul(e.T,np.matmul(self.P,e)) / self.clf.epsilon 56 | + 0.5*self.trGssGP) 57 | 58 | # build constraints for barriers 59 | N_cbf = len(self.cbf_list) 60 | G_cbf = np.zeros((N_cbf,self.xdim/2+2), dtype=np.float32) 61 | h_cbf = np.zeros((N_cbf,1), dtype=np.float32) 62 | A0x_Gmud = np.matmul(self.A0,x[:-1,:]) + np.matmul(self.G,mu_d) 63 | GssG_22 = GssG[2:,2:] 64 | for i, cbf in enumerate(self.cbf_list): 65 | h_x, dB, d2B = cbf.get_B_derivatives(x) 66 | G_cbf[i,:] = np.append(np.append(np.einsum('ij,jk',dB,self.G),0),1) 67 | trGssGd2B = np.einsum('ii',np.einsum('ij,jk',GssG_22,d2B)) 68 | h_cbf[i,:] = -1 * (np.einsum('ij,jk',dB,A0x_Gmud) 69 | - cbf.gamma * h_x 70 | + 0.5*trGssGd2B) 71 | 72 | # build constraints for control limits 73 | ginv = np.linalg.inv(self.dyn.g(x)) 74 | l_ctrl = np.matmul(ginv, mu_d - self.dyn.f(x)) 75 | A_ctrl = ginv 76 | 77 | G_ctrl = np.zeros((self.udim*2,self.xdim/2+2), dtype=np.float32) 78 | h_ctrl = np.zeros((self.udim*2,1), dtype=np.float32) 79 | for i in range(self.udim): 80 | G_ctrl[i*2,:self.xdim/2] = - A_ctrl[i,:] 81 | h_ctrl[i*2] = - self.u_lim[i,0] + l_ctrl[i] 82 | G_ctrl[i*2+1,:self.xdim/2] = A_ctrl[i,:] 83 | h_ctrl[i*2+1] = self.u_lim[i,1] - l_ctrl[i] 84 | 85 | # stack into one matrix and vector 86 | G = np.concatenate((G_dyn,G_cbf,G_ctrl),axis=0) 87 | h = np.concatenate((h_dyn,h_cbf,h_ctrl),axis=0) 88 | 89 | self.G_csc = sparse.csc_matrix(G) 90 | self.h = h 91 | 92 | # dummy lower bound 93 | l = np.ones(h.shape, dtype=np.float32)*np.inf * -1 94 | 95 | #Solve QP 96 | self.prob = osqp.OSQP() 97 | exception_called = False 98 | mu_bar = np.zeros((self.xdim+1), dtype=np.float32) 99 | # try: 100 | self.prob.setup(P=self.Q, q=self.p, A=self.G_csc, l=l, u=self.h, verbose=self.verbose) 101 | self.res = self.prob.solve() 102 | # except: 103 | # exception_called = True 104 | # else: 105 | mu_bar = self.res.x 106 | # if exception_called or u_bar[0] is None or np.isnan(u_bar).any(): 107 | if mu_bar[0] is None or np.isnan(mu_bar).any(): 108 | mu_bar = np.zeros((self.xdim+1)) 109 | self.res = None 110 | rospy.logwarn("QP failed!") 111 | 112 | 113 | self.mu_qp_prev = np.expand_dims(mu_bar[0:self.xdim/2],axis=0).T 114 | 115 | self.V =self.clf.V(x,x_d) 116 | 117 | if self.verbose: 118 | print('z_ref: ', x.T) 119 | print('z_des: ', x_d.T) 120 | print('u_lim', self.u_lim) 121 | print('V: ', self.V) 122 | print('Q:', Q) 123 | print('p:', np.array(self.p)) 124 | print('G_dyn:', G_dyn) 125 | print('h_dyn:', h_dyn) 126 | print('trGssGP',self.trGssGP) 127 | if h_cbf.shape[0] < 10: 128 | print('G_cbf:', G_cbf) 129 | print('h_cbf:', h_cbf) 130 | print('G_ctrl:', G_ctrl) 131 | print('h_ctrl:', h_ctrl) 132 | print('result:', mu_bar) 133 | 134 | return self.mu_qp_prev 135 | -------------------------------------------------------------------------------- /src/scaledgp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import GPy 4 | 5 | 6 | class ScaledGP: 7 | def __init__(self,xdim=1,ydim=1): 8 | self.xdim=xdim 9 | self.ydim=ydim 10 | self.ystd = np.ones(ydim) 11 | self.ymean = np.zeros(ydim) 12 | self.xstd = np.ones(xdim) 13 | self.xmean = np.zeros(xdim) 14 | self.m = GPy.models.GPRegression(np.zeros((1,xdim)),np.zeros((1,ydim))) 15 | 16 | def optimize(self,x,y,update_scaling=True, num_inducing=50): 17 | assert(x.shape[1] == self.xdim and y.shape[1] == self.ydim) 18 | assert(x.shape[0] > 0 and y.shape[0] > 0) 19 | 20 | xmean = self.xmean 21 | xstd = self.xstd 22 | ymean = self.ymean 23 | ystd = self.ystd 24 | if update_scaling: 25 | xmean,xstd = self.update_xscale(x) 26 | ymean,ystd = self.update_yscale(y) 27 | 28 | x = self.scalex(x,xmean,xstd) 29 | y = self.scaley(y,ymean,ystd) 30 | updated_model = GPy.models.GPRegression(x,y) 31 | # self.m = GPy.models.SparseGPRegression(x,y,num_inducing=num_inducing) 32 | updated_model.optimize('bfgs') 33 | self.m = updated_model 34 | 35 | self.xmean = xmean 36 | self.xstd = xstd 37 | self.ymean = ymean 38 | self.ystd = ystd 39 | 40 | def predict(self,x): 41 | x = self.scalex(x,self.xmean,self.xstd) 42 | mean,var = self.m.predict_noiseless(x) 43 | mean = self.unscaley(mean,self.ymean,self.ystd) 44 | var = var * self.ystd 45 | if mean.size == 1: 46 | mean = mean[0,0] 47 | var = var[0,0] 48 | return mean,var 49 | 50 | def update_xscale(self,x): 51 | xmean = np.mean(x,axis=0) 52 | xstd = np.std(x,axis=0) 53 | 54 | return xmean,xstd 55 | 56 | def update_yscale(self,y): 57 | ymean = np.mean(y,axis=0) 58 | ystd = np.std(y,axis=0) 59 | 60 | return ymean, ystd 61 | 62 | def scalex(self,x,xmean,xstd): 63 | if (xstd == 0).any(): 64 | return (x-xmean) 65 | else: 66 | return (x - xmean) / xstd 67 | 68 | def scaley(self,y,ymean,ystd): 69 | if (ystd == 0).any(): 70 | return (y-ymean) 71 | else: 72 | return (y - ymean) / ystd 73 | 74 | def unscalex(self,x,xmean,xstd): 75 | if (xstd == 0).any(): 76 | return x + xmean 77 | else: 78 | return x * xstd + xmean 79 | 80 | def unscaley(self,y,ymean,ystd): 81 | if (ystd == 0).any(): 82 | return y + ymean 83 | else: 84 | return y * ystd + ymean -------------------------------------------------------------------------------- /src/test_adaptive_clbf.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | import copy 5 | from adaptive_clbf import AdaptiveClbf 6 | from dynamics import DynamicsAckermannZModified 7 | from progress.bar import Bar 8 | import time 9 | import matplotlib 10 | import matplotlib.pyplot as plt 11 | matplotlib.rcParams['pdf.fonttype'] = 42 12 | matplotlib.rcParams['ps.fonttype'] = 42 13 | 14 | odim = 2 15 | np.random.seed(0) 16 | adaptive_clbf = AdaptiveClbf(odim=odim, use_service = False) 17 | adaptive_clbf_qp = AdaptiveClbf(odim=odim, use_service = False) 18 | adaptive_clbf_ad = AdaptiveClbf(odim=odim, use_service = False) 19 | adaptive_clbf_pd = AdaptiveClbf(odim=odim, use_service = False) 20 | 21 | params={} 22 | params["vehicle_length"] = 0.25 23 | params["steering_limit"] = 0.75 24 | params["max_accel"] = 1.0 25 | params["min_accel"] = -1.0 26 | params["kp_z"] = 1.0 27 | params["kd_z"] = 1.0 28 | params["clf_epsilon"] = 100.0 29 | 30 | 31 | params["qp_u_cost"] = 100.0 32 | params["qp_u_prev_cost"] = 1.0 33 | params["qp_p1_cost"] = 1.0 34 | params["qp_p2_cost"] = 1.0e12 35 | params["qp_max_var"] = 1.5 36 | params["qp_verbose"] = False 37 | params["max_velocity"] = 2.0 38 | params["min_velocity"] = 0.5 39 | params["barrier_vel_gamma"] = 10.0 40 | params["use_barrier_vel"] = True 41 | params["use_barrier_pointcloud"] = True 42 | params["barrier_radius"] = 1.0 43 | params["barrier_radius_velocity_scale"] = 0.0 44 | params["barrier_pc_gamma_p"] = 5.0 45 | params["barrier_pc_gamma"] = 1.0 46 | params["verbose"] = False 47 | params["dt"] = 0.1 48 | params["max_error"] = 10.0 49 | 50 | # alpaca params 51 | # params["qp_ksig"] = 2.0e3 52 | # params["measurement_noise"] = 1.0e-3 53 | 54 | # gp params 55 | # params["qp_ksig"] = 1.0e5 56 | # params["measurement_noise"] = 1.0 57 | 58 | # vanilla nn params 59 | params["qp_ksig"] = 1.0e2 60 | params["measurement_noise"] = 1.0 61 | 62 | params["N_data"] = 600 63 | params["learning_verbose"] = False 64 | params["N_updates"] = 50 65 | params["meta_batch_size"] = 50 66 | params["data_horizon"] = 20 67 | params["test_horizon"] = 30 68 | params["learning_rate"] = 0.001 69 | params["min_datapoints"] = 50 70 | params["save_data_interval"] = 10000 71 | 72 | true_dyn = DynamicsAckermannZModified(disturbance_scale_pos = 0.0, disturbance_scale_vel = -1.0, control_input_scale = 1.0) 73 | 74 | adaptive_clbf.update_params(params) 75 | adaptive_clbf_qp.update_params(params) 76 | adaptive_clbf_ad.update_params(params) 77 | adaptive_clbf_pd.update_params(params) 78 | adaptive_clbf.true_dyn = true_dyn 79 | adaptive_clbf_ad.true_dyn = true_dyn 80 | 81 | barrier_x = np.array([5,15,25,35,45,55]) 82 | barrier_y = np.array([0,-0.5,0.5,-0.5,0.5,0]) 83 | # barrier_x = np.linspace(0,1,100) 84 | # barrier_y = np.linspace(0,1,100) 85 | # barrier_x = np.array([]) 86 | # barrier_y = np.array([]) 87 | adaptive_clbf.update_barrier_locations(barrier_x,barrier_y,params["barrier_radius"]) 88 | adaptive_clbf_qp.update_barrier_locations(barrier_x,barrier_y,params["barrier_radius"]) 89 | adaptive_clbf_ad.update_barrier_locations(barrier_x,barrier_y,params["barrier_radius"]) 90 | adaptive_clbf_pd.update_barrier_locations(barrier_x,barrier_y,params["barrier_radius"]) 91 | 92 | x0=np.array([[0.0],[0.0],[0.0],[0.0001]]) 93 | z0 = true_dyn.convert_x_to_z(x0) 94 | 95 | T = 40 96 | dt = 0.1 97 | N = int(round(T/dt)) 98 | t = np.linspace(0,T-2*dt,N-1) 99 | xdim=4 100 | udim=2 101 | 102 | train_interval = 40 103 | start_training = 100 104 | 105 | width = 1.0 106 | speed = 1.0 107 | freq = 1.0/10 108 | x_d = np.stack((t * speed, width * np.sin(2 * np.pi * t * freq),np.zeros(N-1), np.zeros(N-1))) 109 | x_d[2,:-1] = np.arctan2(np.diff(x_d[1,:]),np.diff(x_d[0,:])) 110 | x_d[3,:-1] = np.sqrt(np.diff(x_d[0,:])**2 + np.diff(x_d[1,:])**2)/dt 111 | x_d[2,-1]=x_d[2,-2] 112 | x_d[3,-1]=x_d[3,-2] 113 | 114 | u = np.zeros((udim,N)) 115 | x = np.zeros((xdim,N-1)) 116 | x[:,0:1] = x0 117 | 118 | u_qp = np.zeros((udim,N)) 119 | x_qp = np.zeros((xdim,N-1)) 120 | x_qp[:,0:1] = x0 121 | 122 | u_pd = np.zeros((udim,N)) 123 | x_pd = np.zeros((xdim,N-1)) 124 | x_pd[:,0:1] = x0 125 | 126 | u_ad = np.zeros((udim,N)) 127 | x_ad = np.zeros((xdim,N-1)) 128 | x_ad[:,0:1] = x0 129 | 130 | z_d = np.zeros((xdim+1,N-1)) 131 | z = np.zeros((xdim+1,N-1)) 132 | z[:,0:1] = z0 133 | z_qp = np.zeros((xdim+1,N-1)) 134 | z_qp[:,0:1] = z0 135 | z_pd = np.zeros((xdim+1,N-1)) 136 | z_pd[:,0:1] = z0 137 | z_ad = np.zeros((xdim+1,N-1)) 138 | z_ad[:,0:1] = z0 139 | 140 | z_d_dot = np.zeros((xdim+1,1)) 141 | 142 | prediction_error_ad = np.zeros(N) 143 | prediction_error_true_ad = np.zeros(N) 144 | prediction_error = np.zeros(N) 145 | prediction_error_true = np.zeros(N) 146 | prediction_var = np.zeros((xdim/2,N)) 147 | prediction_var_ad = np.zeros((xdim/2,N)) 148 | trGssGP = np.zeros(N) 149 | 150 | i=0 151 | z_d[:,i+1:i+2] = true_dyn.convert_x_to_z(x_d[:,i+1:i+2]) 152 | 153 | bar = Bar(max=N-1) 154 | for i in range(N-2): 155 | bar.next() 156 | start = time.time() 157 | 158 | if i < N-3: 159 | z_d[:,i+2:i+3] = true_dyn.convert_x_to_z(x_d[:,i+2:i+3]) 160 | z_d_dot = (z_d[:,i+2:i+3] - z_d[:,i+1:i+2])/dt 161 | 162 | if i == 0: 163 | add_data = False 164 | else: 165 | add_data = True 166 | 167 | u[:,i+1] = adaptive_clbf.get_control(z[:,i:i+1],z_d[:,i+1:i+2],z_d_dot,dt=dt,obs=np.concatenate([x_ad[2,i:i+1],u_ad[:,i]]),use_model=True,add_data=add_data,use_qp=True) 168 | if (i - start_training -1 ) % train_interval == 0 and i > start_training: 169 | adaptive_clbf.model.train() 170 | adaptive_clbf.model_trained = True 171 | prediction_error[i] = adaptive_clbf.predict_error 172 | prediction_error_true[i] = adaptive_clbf.true_predict_error 173 | prediction_var[:,i:i+1] = np.clip(adaptive_clbf.predict_var,0,params["qp_max_var"]) 174 | trGssGP[i] = adaptive_clbf.qpsolve.trGssGP 175 | 176 | u_ad[:,i+1] = adaptive_clbf_ad.get_control(z_ad[:,i:i+1],z_d[:,i+1:i+2],z_d_dot,dt=dt,obs=np.concatenate([x_ad[2,i:i+1],u_ad[:,i]]),use_model=True,add_data=add_data,use_qp=False) 177 | if (i - start_training - 1) % train_interval == 0 and i > start_training: 178 | adaptive_clbf_ad.model.train() 179 | adaptive_clbf_ad.model_trained = True 180 | prediction_error_ad[i] = adaptive_clbf_ad.predict_error 181 | prediction_error_true_ad[i] = adaptive_clbf_ad.true_predict_error 182 | prediction_var_ad[:,i:i+1] = np.clip(adaptive_clbf_ad.predict_var,0,params["qp_max_var"]) 183 | 184 | u_qp[:,i+1] = adaptive_clbf_qp.get_control(z_qp[:,i:i+1],z_d[:,i+1:i+2],z_d_dot,dt=dt,obs=[],use_model=False,add_data=False,use_qp=True) 185 | 186 | u_pd[:,i+1] = adaptive_clbf_pd.get_control(z_pd[:,i:i+1],z_d[:,i+1:i+2],z_d_dot,dt=dt,obs=[],use_model=False,add_data=False,use_qp=False) 187 | 188 | # dt = np.random.uniform(0.05,0.15) 189 | c = copy.copy(u[:,i+1:i+2]) 190 | c_ad = copy.copy(u_ad[:,i+1:i+2]) 191 | c_qp = copy.copy(u_qp[:,i+1:i+2]) 192 | c_pd = copy.copy(u_pd[:,i+1:i+2]) 193 | 194 | c[0] = np.tan(c[0])/params["vehicle_length"] 195 | c_ad[0] = np.tan(c_ad[0])/params["vehicle_length"] 196 | c_qp[0] = np.tan(c_qp[0])/params["vehicle_length"] 197 | c_pd[0] = np.tan(c_pd[0])/params["vehicle_length"] 198 | 199 | z[:,i+1:i+2] = true_dyn.step(z[:,i:i+1],c,dt) 200 | z_ad[:,i+1:i+2] = true_dyn.step(z_ad[:,i:i+1],c_ad,dt) 201 | z_qp[:,i+1:i+2] = true_dyn.step(z_qp[:,i:i+1],c_qp,dt) 202 | z_pd[:,i+1:i+2] = true_dyn.step(z_pd[:,i:i+1],c_pd,dt) 203 | 204 | x[:,i+1:i+2] = true_dyn.convert_z_to_x(z[:,i+1:i+2]) 205 | x_ad[:,i+1:i+2] = true_dyn.convert_z_to_x(z_ad[:,i+1:i+2]) 206 | x_qp[:,i+1:i+2] = true_dyn.convert_z_to_x(z_qp[:,i+1:i+2]) 207 | x_pd[:,i+1:i+2] = true_dyn.convert_z_to_x(z_pd[:,i+1:i+2]) 208 | 209 | print('Iteration ', i, ', Time elapsed (ms): ', (time.time() - start)*1000) 210 | 211 | 212 | plt.figure() 213 | plt.rcParams.update({'font.size': 12}) 214 | plt.subplot(311) 215 | plt.semilogy(t[:-1],prediction_var_ad[0,:-2],'m--',alpha=0.9) 216 | plt.semilogy(t[:-1],prediction_var[0,:-2],'g-',alpha=0.9) 217 | plt.semilogy(t[:-1],prediction_var_ad[1,:-2],'m:',alpha=0.9) 218 | plt.semilogy(t[:-1],prediction_var[1,:-2],'g:',alpha=0.9) 219 | plt.xlabel("Time(s)") 220 | plt.ylabel(r"$\sigma_{\bar{\Delta}}(x,\mu)$") 221 | plt.legend(['ad[1]','aclbf[1]','ad[2]','aclbf[2]'],bbox_to_anchor=(0,1.2,1,0.2), loc="upper center", ncol=2) 222 | plt.plot([t[0],t[-1]],[params["measurement_noise"],params["measurement_noise"]],'r--') 223 | plt.subplot(312) 224 | plt.plot(t[:-1],trGssGP[:-2],'g--',alpha=0.9) 225 | plt.ylabel("trace(GssGP)") 226 | plt.xlabel("Time(s)") 227 | plt.subplot(313) 228 | plt.plot(t[:-1],prediction_error_ad[:-2],'m--',alpha=0.9) 229 | plt.plot(t[:-1],prediction_error[:-2],'g-',alpha=0.9) 230 | plt.plot(t[:-1],prediction_error_true_ad[:-2],'m:',alpha=0.9) 231 | plt.plot(t[:-1],prediction_error_true[:-2],'g:',alpha=0.9) 232 | # plt.ylim([0,1.0]) 233 | plt.ylabel("Prediction error") 234 | plt.xlabel("Time(s)") 235 | 236 | 237 | fig = plt.figure() 238 | plt.rcParams.update({'font.size': 12}) 239 | plt.plot(x_d[0,:],x_d[1,:],'k-',label='ref') 240 | plt.plot(x_ad[0,:],x_ad[1,:],'m--',alpha=0.9,label='ad') 241 | plt.plot(x_qp[0,:],x_qp[1,:],'b-',alpha=0.9,label='qp') 242 | plt.plot(x_pd[0,:],x_pd[1,:],'y:',alpha=0.9,label='pd') 243 | plt.plot(x[0,:],x[1,:],'g-',alpha=0.9,label='aclbf',linewidth=3.0) 244 | plt.legend(bbox_to_anchor=(0,1.02,1,0.2), loc="lower center", ncol=5) 245 | ax = fig.gca() 246 | for i in range(barrier_x.size): 247 | circle = plt.Circle((barrier_x[i],barrier_y[i]), params["barrier_radius"], color='r') 248 | ax.add_artist(circle) 249 | plt.xlabel('X Position') 250 | plt.ylabel('Y Position') 251 | 252 | 253 | plt.figure() 254 | plt.rcParams.update({'font.size': 12}) 255 | plt.subplot(211) 256 | plt.plot(t,u_ad[0,:-1],'m--',alpha=0.9) 257 | plt.plot(t,u_qp[0,:-1],'b-',alpha=0.9) 258 | plt.plot(t,u_pd[0,:-1],'y:',alpha=0.9) 259 | plt.plot(t,u[0,:-1],'g-',alpha=0.9) 260 | plt.legend(['ad','qp','pd','aclbf'],bbox_to_anchor=(0,1.1,1,0.2), loc="upper center", ncol=4) 261 | plt.plot([t[0],t[-1]],[params["steering_limit"],params["steering_limit"]],'r--') 262 | plt.plot([t[0],t[-1]],[-params["steering_limit"],-params["steering_limit"]],'r--') 263 | plt.ylabel('Steering Angle (rad)') 264 | plt.subplot(212) 265 | plt.plot(t,u_ad[1,:-1],'m--',alpha=0.9) 266 | plt.plot(t,u_qp[1,:-1],'b-',alpha=0.9) 267 | plt.plot(t,u_pd[1,:-1],'y:',alpha=0.9) 268 | plt.plot(t,u[1,:-1],'g-',alpha=0.9) 269 | plt.plot([t[0],t[-1]],[params["min_accel"],params["min_accel"]],'r--') 270 | plt.plot([t[0],t[-1]],[params["max_accel"],params["max_accel"]],'r--') 271 | plt.ylabel('Throttle (m/s^2)') 272 | plt.xlabel('Time (s)') 273 | 274 | 275 | barrier_dist_ad = np.min(np.stack([np.sqrt((barrier_x[i]-x_ad[0,:])**2 + (barrier_y[i]-x_ad[1,:])**2) for i in range(len(barrier_x))]),axis=0) 276 | barrier_dist_qp = np.min(np.stack([np.sqrt((barrier_x[i]-x_qp[0,:])**2 + (barrier_y[i]-x_qp[1,:])**2) for i in range(len(barrier_x))]),axis=0) 277 | barrier_dist_pd = np.min(np.stack([np.sqrt((barrier_x[i]-x_pd[0,:])**2 + (barrier_y[i]-x_pd[1,:])**2) for i in range(len(barrier_x))]),axis=0) 278 | barrier_dist = np.min(np.stack([np.sqrt((barrier_x[i]-x[0,:])**2 + (barrier_y[i]-x[1,:])**2) for i in range(len(barrier_x))]),axis=0) 279 | 280 | plt.figure() 281 | plt.rcParams.update({'font.size': 12}) 282 | plt.subplot(311) 283 | plt.plot(t,np.sqrt((x_d[0,:]-x_ad[0,:])**2 + (x_d[1,:]-x_ad[1,:])**2),'m--',alpha=0.9) 284 | plt.plot(t,np.sqrt((x_d[0,:]-x_qp[0,:])**2 + (x_d[1,:]-x_qp[1,:])**2),'b-',alpha=0.9) 285 | plt.plot(t,np.sqrt((x_d[0,:]-x_pd[0,:])**2 + (x_d[1,:]-x_pd[1,:])**2),'y:',alpha=0.9) 286 | plt.plot(t,np.sqrt((x_d[0,:]-x[0,:])**2 + (x_d[1,:]-x[1,:])**2),'g-',alpha=0.9) 287 | plt.plot([0],[0],'r--') 288 | plt.ylabel("Pos error (m)") 289 | plt.legend(['ad','qp','pd','aclbf','barrier'],bbox_to_anchor=(0,1.2,1,0.2), loc="upper center", ncol=5) 290 | # plt.ylim([0,1.0]) 291 | plt.subplot(312) 292 | plt.plot(t,x_d[3,:],'k-') 293 | plt.plot(t,x_ad[3,:],'m--',alpha=0.9) 294 | plt.plot(t,x_qp[3,:],'b-',alpha=0.9) 295 | plt.plot(t,x_pd[3,:],'y:',alpha=0.9) 296 | plt.plot(t,x[3,:],'g-',alpha=0.9) 297 | plt.ylabel('Vel (m/s)') 298 | plt.plot([t[0],t[-1]],[params["max_velocity"],params["max_velocity"]],'r--') 299 | plt.plot([t[0],t[-1]],[params["min_velocity"],params["min_velocity"]],'r--') 300 | plt.xlabel('Time (s)') 301 | plt.subplot(313) 302 | plt.plot(t,barrier_dist_ad-params["barrier_radius"],'m--',alpha=0.9) 303 | plt.plot(t,barrier_dist_qp-params["barrier_radius"],'b-',alpha=0.9) 304 | plt.plot(t,barrier_dist_pd-params["barrier_radius"],'y:',alpha=0.9) 305 | plt.plot(t,barrier_dist-params["barrier_radius"],'g-',alpha=0.9) 306 | plt.plot([t[0],t[-1]],[0,0],'r--') 307 | plt.ylabel("Dist to barrier (m)") 308 | plt.xlabel("Time(s)") 309 | 310 | print("mean prediction error: ", np.mean(prediction_error[start_training+2:])) 311 | print("mean true error: ", np.mean(prediction_error_true[start_training+2:])) 312 | print("mean position error: ", np.mean(np.sqrt((x_d[0,:]-x[0,:])**2 + (x_d[1,:]-x[1,:])**2))) 313 | 314 | print("mean prediction error (ad): ", np.mean(prediction_error_ad[start_training+2:])) 315 | print("mean true error (ad): ", np.mean(prediction_error_true_ad[start_training+2:])) 316 | print("mean position error (ad): ", np.mean(np.sqrt((x_d[0,:]-x_ad[0,:])**2 + (x_d[1,:]-x_ad[1,:])**2))) 317 | 318 | plt.show() 319 | -------------------------------------------------------------------------------- /src/vanilla_nn.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import keras 3 | from keras.models import Model 4 | from keras.layers import Input, Layer, Dense, Dropout, Activation 5 | from keras import backend as K 6 | 7 | class NegativeLogLikelihood(Layer): 8 | def call(self, inputs): 9 | y_pred_mean = inputs[0] 10 | y_pred_var = inputs[1] 11 | y_true = inputs[2] 12 | loss = K.sum(K.log(y_pred_var+K.epsilon())+K.square(y_pred_mean-y_true) / y_pred_var,axis=-1) 13 | return loss 14 | 15 | def identity_loss(y_true, loss): 16 | return loss 17 | 18 | class VanillaNN: 19 | def __init__(self,xdim,ydim): 20 | input_x = Input(shape=(xdim,), dtype='float', name='input_x') 21 | x = Dense(256, activation='tanh')(input_x) 22 | x = Dropout(0.5)(x) 23 | x = Dense(256, activation='tanh')(x) 24 | x = Dropout(0.5)(x) 25 | x = Dense(256, activation='tanh')(x) 26 | x = Dropout(0.5)(x) 27 | x = Dense(256, activation='tanh')(x) 28 | output_mean = Dense(ydim, activation='linear', name='output_mean')(x) 29 | output_var = Dense(ydim, activation='softplus', name='output_var')(x) 30 | input_y = Input(shape=(ydim,), dtype='float', name='input_y') 31 | output_loss = NegativeLogLikelihood()([output_mean,output_var,input_y]) 32 | 33 | self.m = Model(inputs=[input_x,input_y], outputs=[output_mean,output_var,output_loss]) 34 | rmsprop = keras.optimizers.RMSprop(lr=0.001, rho=0.9, epsilon=None, decay=0.0) 35 | self.m.compile(loss=identity_loss, loss_weights=[0., 0., 1.], optimizer=rmsprop) 36 | 37 | self.graph = tf.get_default_graph() 38 | 39 | def train(self, Z_train, Y_train,_epoch=50,_batch_size=64,_verbose=0): 40 | with self.graph.as_default(): 41 | self.m.fit([Z_train,Y_train], [Y_train,Y_train,Y_train],epochs=_epoch,batch_size=_batch_size,verbose=_verbose) 42 | 43 | def predict(self, ZN, Y, _batch_size=1): 44 | with self.graph.as_default(): 45 | y, var, loss = self.m.predict([ZN,Y], batch_size=_batch_size) 46 | # print("Predicted y ", y) 47 | # print("predicted var ", var) 48 | # print("predicted loss ", loss) 49 | return y[0], var[0], loss[0] 50 | -------------------------------------------------------------------------------- /srv/AddData2Model.srv: -------------------------------------------------------------------------------- 1 | float32[] x_next 2 | float32[] x 3 | float32[] mu_model 4 | float32[] obs 5 | float32 dt 6 | --- 7 | # result 8 | bool done 9 | -------------------------------------------------------------------------------- /srv/PredictModel.srv: -------------------------------------------------------------------------------- 1 | float32[] x 2 | float32[] obs 3 | --- 4 | # result 5 | float32[] y_out 6 | float32[] var 7 | bool result 8 | --------------------------------------------------------------------------------