├── .gitignore ├── .gitlab-ci.yml ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── analysis ├── impulse_force.bag ├── make_plots.py ├── notebooks │ ├── CgPlacement │ │ ├── CgAnalysis.ipynb │ │ └── env.py │ ├── DriveControl │ │ ├── DriveControlAnalysis.ipynb │ │ ├── env.py │ │ └── plots.yaml │ ├── FilterDesign │ │ ├── FilterDesign.ipynb │ │ ├── env.py │ │ └── plots.yaml │ ├── RosJupyterAnalysis │ │ ├── RosJupyterAnalysisPart1.ipynb │ │ ├── RosJupyterAnalysisPart2.ipynb │ │ ├── RosJupyterAnalysisPart3.ipynb │ │ ├── data │ │ │ └── sample_data.zip │ │ ├── nb_env.py │ │ └── plots.yaml │ ├── TiltControl │ │ ├── TiltControlAnalysis.ipynb │ │ ├── env.py │ │ └── plots.yaml │ ├── TurningControl │ │ ├── TurningControlAnalysis.ipynb │ │ ├── env.py │ │ └── plots.yaml │ └── VelocityControl │ │ ├── VelocityControlAnalysis.ipynb │ │ ├── env.py │ │ └── plots.yaml └── plots.yaml ├── bobble_controllers_plugins.xml ├── config └── bobble_sim_balance_control.yaml ├── docs └── imgs │ ├── BobbleBotGazebo.png │ ├── BobbleCAD.png │ ├── DifferentialDrive.png │ ├── JoystickControls.png │ ├── TiltControl.png │ └── VelocityControl.png ├── include └── bobble_controllers │ ├── BalanceBaseController.h │ ├── BalanceControllerData.h │ ├── BalanceSimController.h │ ├── Filter.h │ ├── HighPassFilter.h │ ├── LowPassFilter.h │ ├── MadgwickAHRS.h │ ├── PIDFilters.h │ └── PidControl.h ├── launch ├── apply_impulse_force.launch ├── bobble_balance_state_hold.launch ├── drive_square.launch ├── drive_square_js_cmds.bag ├── run_sim.launch └── run_sim_with_joystick.launch ├── msg ├── BobbleBotStatus.msg └── ControlCommands.msg ├── package.xml ├── setup.py ├── src ├── BalanceBaseController.cpp ├── BalanceSimController.cpp ├── Filter.cpp ├── HighPassFilter.cpp ├── LowPassFilter.cpp ├── MadgwickAHRS.cpp ├── PIDFilters.cpp ├── PidControl.cpp ├── analysis_tools │ ├── __init__.py │ ├── filters.py │ ├── parsing.py │ └── plots.py └── nodes │ ├── ApplyImpulse │ ├── BobbleStateHold │ ├── JoystickControl │ ├── KeyboardControl │ └── __init__.py └── test ├── BobbleIntegrationTest.py ├── TestSummary.ipynb ├── bad_balance_control_config.yaml ├── balance_controller.test ├── balance_controller_test.cpp ├── common.launch ├── data ├── balance.bag ├── balance.csv ├── drive_square_js_response.bag ├── drive_square_js_response.csv ├── impulse_force.bag ├── impulse_force.csv ├── no_balance.bag └── no_balance.csv ├── filter_test.cpp ├── good_balance_control_config.yaml ├── impulse_force.test ├── js_drive_square.test ├── nb_env.py ├── no_balance.test ├── no_balance_test.cpp ├── parsing_test.py ├── plots.yaml ├── plots_test.py ├── test_common.h ├── test_summary_plots.yaml └── xacro ├── bno055_imu.urdf.xacro ├── bobble.urdf.xacro ├── bobble_chassis.urdf.xacro ├── bobble_world.urdf.xacro ├── left_wheel.urdf.xacro └── right_wheel.urdf.xacro /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.swo 3 | bin 4 | build 5 | msg_gen 6 | srv_gen 7 | cfg/*.cfgc 8 | cfg/cpp 9 | site 10 | *.pyc 11 | utils/data/* 12 | test/data/* 13 | *.bag 14 | *.ipynb_checkpoints 15 | *.zip 16 | analysis/notebooks/*data 17 | *.png 18 | test/TestSummary.html 19 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: superowesome/bobble-sim:stable 2 | 3 | stages: 4 | - build 5 | - test 6 | - analyze 7 | 8 | cache: 9 | paths: 10 | - ccache/ 11 | 12 | before_script: 13 | - source /opt/ros/melodic/setup.bash 14 | - git clone https://gitlab.com/VictorLamoine/ros_gitlab_ci.git 15 | - source ros_gitlab_ci/gitlab-ci.bash || true 16 | 17 | catkin_lint: 18 | stage: build 19 | before_script: 20 | - apt update -q -y || true 21 | - apt install -q -y python-catkin-lint || true 22 | script: 23 | - catkin_lint -W3 . 24 | 25 | # catkin tools 26 | catkin_tools: 27 | stage: build 28 | script: 29 | - catkin build --summarize --no-status --force-color 30 | 31 | # catkin_make 32 | catkin_make: 33 | stage: build 34 | script: 35 | - catkin_make 36 | 37 | catkin_tools_tests: 38 | stage: test 39 | script: 40 | - catkin build --summarize --no-status --force-color 41 | - catkin run_tests --no-status --force-color -j1 42 | - mv src/bobble_controllers/test/data/*.csv .. 43 | - catkin_test_results --verbose . 44 | artifacts: 45 | paths: 46 | - ./*.csv 47 | 48 | # These move commands should not be neccessary and break 49 | # running these tests locally. It is all working around 50 | # a limitation of properly passing artifacts 51 | # from one stage to the next. 52 | # Open issue on this is here : 53 | # https://gitlab.com/VictorLamoine/ros_gitlab_ci/issues/5 54 | test_summary_report: 55 | stage: analyze 56 | script: 57 | - mv ../*.csv src/bobble_controllers/test/data/ 58 | - catkin config --install 59 | - catkin build 60 | - source install/setup.bash 61 | - cd src/bobble_controllers/test 62 | - jupyter nbconvert --execute --to html TestSummary.ipynb 63 | - cd ../../.. 64 | - mv src/bobble_controllers/test/TestSummary.html .. 65 | - mv src/bobble_controllers/test/data/*.csv .. 66 | dependencies: 67 | - catkin_tools_tests 68 | artifacts: 69 | paths: 70 | - ./TestSummary.html 71 | - ./*.png 72 | - ./*.csv 73 | 74 | 75 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bobble_controllers) 3 | add_compile_options(-std=c++11) 4 | 5 | find_package( 6 | catkin 7 | REQUIRED COMPONENTS 8 | control_msgs 9 | controller_interface 10 | controller_manager 11 | message_generation 12 | pluginlib 13 | rospy 14 | rostest 15 | sensor_msgs 16 | std_msgs 17 | ) 18 | 19 | catkin_python_setup() 20 | 21 | add_message_files( 22 | FILES 23 | BobbleBotStatus.msg 24 | ControlCommands.msg 25 | ) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | ) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | LIBRARIES ${PROJECT_NAME} 35 | CATKIN_DEPENDS 36 | control_msgs 37 | controller_interface 38 | controller_manager 39 | message_runtime 40 | pluginlib 41 | rospy 42 | rostest 43 | sensor_msgs 44 | std_msgs 45 | ) 46 | 47 | ########### 48 | ## Build ## 49 | ########### 50 | include_directories(include 51 | ${catkin_INCLUDE_DIRS} 52 | ) 53 | 54 | add_library(${PROJECT_NAME} 55 | src/BalanceBaseController.cpp 56 | src/BalanceSimController.cpp 57 | src/Filter.cpp 58 | src/HighPassFilter.cpp 59 | src/LowPassFilter.cpp 60 | src/MadgwickAHRS.cpp 61 | src/PidControl.cpp 62 | src/PIDFilters.cpp 63 | ) 64 | 65 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) 66 | 67 | target_link_libraries(${PROJECT_NAME} 68 | ${catkin_LIBRARIES} 69 | ) 70 | 71 | ############# 72 | ## Testing ## 73 | ############# 74 | if (CATKIN_ENABLE_TESTING) 75 | catkin_add_gtest(filter_test 76 | test/filter_test.cpp) 77 | target_link_libraries(filter_test ${PROJECT_NAME}) 78 | add_rostest_gtest(balance_controller_test 79 | test/balance_controller.test 80 | test/balance_controller_test.cpp) 81 | target_link_libraries(balance_controller_test ${catkin_LIBRARIES}) 82 | add_rostest_gtest(no_balance_test 83 | test/no_balance.test 84 | test/no_balance_test.cpp) 85 | target_link_libraries(no_balance_test ${catkin_LIBRARIES}) 86 | foreach(T 87 | test/js_drive_square.test 88 | test/impulse_force.test) 89 | add_rostest(${T}) 90 | endforeach() 91 | catkin_add_nosetests(test/parsing_test.py) 92 | catkin_add_nosetests(test/plots_test.py) 93 | endif() 94 | 95 | ############# 96 | ## Install ## 97 | ############# 98 | install(TARGETS ${PROJECT_NAME} 99 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 100 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 101 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 102 | ) 103 | 104 | install(DIRECTORY include/${PROJECT_NAME}/ 105 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 106 | ) 107 | 108 | catkin_install_python( 109 | PROGRAMS 110 | src/nodes/BobbleStateHold 111 | src/nodes/JoystickControl 112 | src/nodes/KeyboardControl 113 | src/nodes/ApplyImpulse 114 | DESTINATION 115 | ${CATKIN_PACKAGE_BIN_DESTINATION} 116 | ) 117 | 118 | install(DIRECTORY launch 119 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 120 | 121 | install(DIRECTORY config 122 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 123 | 124 | install(FILES ${PROJECT_NAME}_plugins.xml 125 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 126 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | When contributing to this repository, please first discuss the change you wish to make via issue, 4 | email, or any other method with the owners of this repository before making a change. 5 | 6 | Please note we have a code of conduct, please follow it in all your interactions with the project. 7 | 8 | ## Pull Request Process 9 | 10 | 1. Fork [bobble_controllers]() and [bobble_description]() 11 | 2. Create your feature branch (`git checkout -b feature/fooBar`) 12 | 3. Implement your changes and commit often using meaningful commit messages. 13 | 4. Ensure the tests in bobble_controllers all pass. 14 | 5. Update the README.md if your changes impact what is currently documented. 15 | 6. Commit your changes (`git commit -am 'Add some fooBar'`) 16 | 7. Push to the branch (`git push origin feature/fooBar`) 17 | 8. Create a new Pull Request 18 | 19 | ## Code of Conduct 20 | 21 | ### Our Pledge 22 | 23 | In the interest of fostering an open and welcoming environment, we as 24 | contributors and maintainers pledge to making participation in our project and 25 | our community a harassment-free experience for everyone, regardless of age, body 26 | size, disability, ethnicity, gender identity and expression, level of experience, 27 | nationality, personal appearance, race, religion, or sexual identity and 28 | orientation. 29 | 30 | ### Our Standards 31 | 32 | Examples of behavior that contributes to creating a positive environment 33 | include: 34 | 35 | * Using welcoming and inclusive language 36 | * Being respectful of differing viewpoints and experiences 37 | * Gracefully accepting constructive criticism 38 | * Focusing on what is best for the community 39 | * Showing empathy towards other community members 40 | 41 | Examples of unacceptable behavior by participants include: 42 | 43 | * The use of sexualized language or imagery and unwelcome sexual attention or 44 | advances 45 | * Trolling, insulting/derogatory comments, and personal or political attacks 46 | * Public or private harassment 47 | * Publishing others' private information, such as a physical or electronic 48 | address, without explicit permission 49 | * Other conduct which could reasonably be considered inappropriate in a 50 | professional setting 51 | 52 | ### Our Responsibilities 53 | 54 | Project maintainers are responsible for clarifying the standards of acceptable 55 | behavior and are expected to take appropriate and fair corrective action in 56 | response to any instances of unacceptable behavior. 57 | 58 | Project maintainers have the right and responsibility to remove, edit, or 59 | reject comments, commits, code, wiki edits, issues, and other contributions 60 | that are not aligned to this Code of Conduct, or to ban temporarily or 61 | permanently any contributor for other behaviors that they deem inappropriate, 62 | threatening, offensive, or harmful. 63 | 64 | ### Scope 65 | 66 | This Code of Conduct applies both within project spaces and in public spaces 67 | when an individual is representing the project or its community. Examples of 68 | representing a project or community include using an official project e-mail 69 | address, posting via an official social media account, or acting as an appointed 70 | representative at an online or offline event. 71 | 72 | ### Enforcement 73 | 74 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 75 | reported by contacting the project team [here](mailto:support@so.engineering). 76 | All complaints will be reviewed and investigated and will result in a response that 77 | is deemed necessary and appropriate to the circumstances. The project team is 78 | obligated to maintain confidentiality with regard to the reporter of an incident. 79 | 80 | Project maintainers who do not follow or enforce the Code of Conduct in good 81 | faith may face temporary or permanent repercussions as determined by other 82 | members of the project's leadership. 83 | 84 | ### Attribution 85 | 86 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, 87 | available at [http://contributor-covenant.org/version/1/4][version] 88 | 89 | [homepage]: http://contributor-covenant.org 90 | [version]: http://contributor-covenant.org/version/1/4/ 91 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Copyright (c) 2018, S.O. Engineering, LLC 3 | 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 7 | the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 13 | following disclaimer in the documentation and/or other materials provided with the distribution. 14 | 15 | * Neither the name of S.O. Engineering nor the names of its contributors may be used to endorse or promote 16 | products derived from this software without specific prior written permission. 17 | 18 | *THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 19 | INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE 24 | USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.* -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Bobble-Bot Simulator 2 | > Follow the Bobble-Bot project on [Hackaday](https://hackaday.io/project/164992-bobble-bot) 3 | 4 | Bobble-Bot is a modern take on a classical problem in control theory. The robot represents 5 | a unique solution to the well understood problem of control of a two wheeled inverted pendulum. 6 | The source code found in this repository constitutes the robot's balance control logic. The controller comes 7 | with a handy Gazebo simulation which was used extensively during the development 8 | of the robot. The source code is being provided as open-source software in order to help others learn about feedback 9 | control and the Robot Operating System (ROS). 10 | 11 | ### Impulse Response 12 | ![](https://media.giphy.com/media/8TCb7GXefeW54RbMKT/giphy.gif) 13 | 14 | To get started, follow the [installation instructions](#debian-install), or if you're more 15 | of the DIY type, [build the simulation from source](#build-from-source) 16 | and learn how to [build your own Bobble-Bot](#build-your-own). 17 | 18 | ## Debian Install 19 | > Beware this section is still in development. 20 | 21 | For now, please follow the instructions to [build the simulation from source](#build-from-source). 22 | 23 | ## Bobble-Bot on Windows 24 | Bobble-Bot now works on Windows thanks to the Windows Subsystem for Linux (WSL)! If you'd like to work with Bobble-Bot 25 | on Windows, follow [these instructions first](https://jack-kawell.com/2020/06/12/ros-wsl2/). Once you've completed the 26 | basic WSL setup, you can proceed to the instructions in the [build the simulation from source section](#build-from-source). 27 | 28 | ## Build from source 29 | 30 | The BobbleBot simulator requires ROS and Gazebo. Follow the instructions 31 | [here](http://wiki.ros.org/melodic/Installation/Ubuntu) 32 | and install ROS Melodic Desktop Full. Other versions of ROS should also work, 33 | but they are not officially supported at this time. 34 | The simulator also makes use of the [Hector Gazebo](http://wiki.ros.org/hector_gazebo_plugins) 35 | plugins. Those can be installed using the command below. 36 | 37 | ```sh 38 | sudo apt-get install ros-melodic-hector-gazebo-plugins 39 | pip install seaborn rosbag_pandas 40 | ``` 41 | 42 | Before starting the build process, make sure your ROS environment is active. 43 | 44 | ```sh 45 | source /opt/ros/melodic/setup.bash 46 | ``` 47 | 48 | Get the code and build it using catkin. 49 | 50 | ```sh 51 | mkdir -p ~/bobble_workspace/src 52 | cd ~/bobble_workspace/src 53 | catkin_init_workspace 54 | git clone https://github.com/super-owesome/bobble_controllers.git 55 | git clone https://github.com/super-owesome/bobble_description.git 56 | cd .. ; catkin_make 57 | source devel/setup.bash 58 | ``` 59 | 60 | The BobbleBot controller package comes with a set of automated tests. If you're doing development on the 61 | BobbleBot simulator you are expected to ensure that these tests continue to pass. To run these tests, 62 | use the command below. 63 | 64 | ```sh 65 | catkin_make run_tests_bobble_controllers -j1 66 | ``` 67 | 68 | The simulation should now be ready to run. Decide if you want to run using [Keyboard Control](keyoard-control) 69 | or [Joystick Control](joystick-control). Please create an issue if you encountered any problems performing the steps above. 70 | 71 | ### Keyboard Control 72 | Launch the simulation. 73 | 74 | ```sh 75 | roslaunch bobble_controllers run_sim.launch gui:=true 76 | ``` 77 | 78 | In a separate terminal (with the ROS environment sourced) launch the keyboard control node. 79 | 80 | ```sh 81 | source devel/setup.bash 82 | rosrun bobble_controllers KeyboardControl 83 | ``` 84 | 85 | The controls are summarized below. The terminal used to launch the keyboard control node must 86 | have the active focus. Hit space-bar to activate the balance controller. 87 | 88 | ```sh 89 | BobbleBot Keyboard Controller 90 | --------------------------- 91 | Activate/Deactivate command: 92 | Activate/Shutdown: space bar 93 | Moving around: 94 | Forward : w 95 | Backward : s 96 | Left : a 97 | Right : d 98 | Speed Up/Down: 99 | 15% Increase: q 100 | 15% Decrease: e 101 | CTRL-C to quit 102 | ``` 103 | 104 | ### Joystick Control 105 | The bobble_controllers package comes with a Joystick control node that is defaulted with a mapping 106 | that is suitable for an Xbox 1 controller. To use joystick control, follow 107 | [these instructions](https://www.maketecheasier.com/set-up-xbox-one-controller-ubuntu/) 108 | to setup your Xbox 1 controller. Next, make sure you have the [ROS joy package installed](http://wiki.ros.org/joy). 109 | With those two steps out of the way, you can then launch the simulator using the command below. 110 | 111 | ```sh 112 | source devel/setup.bash 113 | roslaunch bobble_controllers run_sim_with_joystick.launch gui:=true 114 | ``` 115 | 116 | The default controls are depicted below: 117 | 118 | ![Joystick Controls](docs/imgs/JoystickControls.png) 119 | 120 | 121 | ## Analyzing Simulation Data 122 | This repository also comes with some sample Python scripts and [Jupyter notebooks](https://jupyter.org/) 123 | that show how to use [Pandas](https://pandas.pydata.org/) to analyze output data 124 | from the simulation. Using the [gazebo-ros](https://github.com/ros-simulation/gazebo_ros_pkgs) 125 | packages, one can log data as the simulator runs and store it in a ROS bag format. 126 | The steps below provide an example of how this is done. 127 | 128 | First, we need to generate some data to analyze. The "apply_impulse_force" launch file 129 | is a good one to start with. Let's apply an impulse to Bobble-Bot in the -X direction 130 | and see how the balance controller holds up. 131 | 132 | ```sh 133 | source devel/setup.bash 134 | roslaunch bobble_controllers apply_impulse_force.launch gui:=true impulse:=-1000 out_file:=~/bobble_workspace/src/bobble_controllers/analysis/impulse_test 135 | ``` 136 | 137 | If all goes well, the Gazebo simulation should launch and you should see Bobble-Bot hold 138 | its balance in spite of the applied impulse. After the test completes, you should see a 139 | newly created impulse_test.bag file in the bobble_controllers/analysis directory. Let's 140 | analyze the data in this bag file. 141 | 142 | ```sh 143 | cd src/bobble_controllers/analysis 144 | python make_plots.py --run impulse_test.bag 145 | ``` 146 | 147 | The script above uses the analysis_tools Python module defined in this repository to 148 | load the data and make the plots. After running the make_plots.py script above, you 149 | should see that two images were created: 'TiltControl.png' & 'VelocityControl.png'. 150 | They should look something like the following: 151 | 152 | ![Tilt Control](docs/imgs/TiltControl.png) 153 | ![Velocity Control](docs/imgs/VelocityControl.png) 154 | 155 | Try changing the gains in config/bobble_sim_balance_control.yaml and then repeating 156 | the steps above to generate and analyze new data. Testing and analyzing your changes 157 | against an applied impulse force is a good practice. The apply impulse launch file 158 | and analysis_tools module are part of the automated tests that are run against the 159 | bobble_controllers ROS package. This is how we ensure the controller remains in 160 | a working state. 161 | 162 | ## Docker Setup 163 | 164 | A Bobble-Bot simulation Docker image is available over on 165 | [Docker Hub](https://cloud.docker.com/u/superowesome/repository/docker/superowesome/bobble-sim). 166 | If you do not know what Docker is, we highly suggest checking it out. Docker can be used 167 | to quickly experiment with the simulation without polluting your system with additional 168 | packages. See this [page](https://docs.docker.com/get-started/) to get started. 169 | This particular image relies on [nvidia-docker2](https://github.com/NVIDIA/nvidia-docker). 170 | For now, an NVidia GPU is required if you want to run the graphics from within the container. 171 | If you only care about running the simulation headless (no graphics), then Docker is the 172 | only requirement. 173 | 174 | Run the simulation with graphics from within a container (master branch version of sim). 175 | 176 | ```sh 177 | docker pull superowesome/bobble-sim:stable 178 | cd ~/bobble_workspace/src 179 | git clone https://github.com/super-owesome/bobble_docker.git 180 | cd bobble_docker 181 | ./run_bobble_sim.bash 182 | ``` 183 | 184 | Enable keyboard control (in a separate terminal). 185 | 186 | ```sh 187 | ./run_keyboard_control.bash 188 | ``` 189 | 190 | Run the simulation headless from within a container (master branch version of sim). 191 | 192 | ```sh 193 | ./run_apply_impulse.bash 194 | ``` 195 | 196 | Note that the run_apply_impulse.bash script mounts the bobble_docker folder into the 197 | container and then directs the apply_impulse launch file to write the output data 198 | to that folder. This is a useful script for generating some sample data for analysis. 199 | 200 | Use the bash script below to launch a container intended for building the simulation 201 | from source. This is a useful container for development. 202 | 203 | ```sh 204 | ./run_dev_container.bash 205 | ``` 206 | 207 | This container mounts the bobble_workspace folder in your host 208 | machine's home directory to /bobble_src in the container and then gives 209 | the user an interactive bash shell. This shell can then be used to build and run 210 | the simulation from within a container. We recommended that you use your favorite 211 | desktop editor to make code modifications, and then just use the container to build 212 | and launch the simulation. Execute commands like the following inside the container 213 | to rebuild the sim from source and run it. 214 | 215 | ```sh 216 | cd /bobble_src 217 | catkin config --install 218 | catkin init 219 | catkin build 220 | source install/setup.bash 221 | roslaunch bobble_controllers apply_impulse_force.launch gui:=true 222 | ``` 223 | 224 | You can now freely edit the files on your host machine and simply re-run catkin 225 | build and roslaunch commands at will. 226 | 227 | 228 | ## Build Your Own 229 | > Vote Bobble-Bot for the [Hackaday Prize 2019](https://hackaday.io/project/164992-bobble-bot) 230 | 231 | Even more details for hobbyists and DIY types can be found on our 232 | [hackaday project page](https://hackaday.io/project/164992-bobble-bot). Check it out! 233 | -------------------------------------------------------------------------------- /analysis/impulse_force.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/analysis/impulse_force.bag -------------------------------------------------------------------------------- /analysis/make_plots.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | matplotlib.use('Agg') 3 | import os, argparse 4 | import matplotlib.pyplot as plt 5 | import seaborn as sns 6 | plt.rcParams['axes.grid'] = True 7 | sns.set_context("poster", font_scale=1.1) 8 | sns.set_palette("Dark2") 9 | 10 | def load_data(sim_data_bag_file, run_name): 11 | # Parse sim data 12 | from analysis_tools.parsing import parse_single_run 13 | print "Loading sim data from run : " + str(run_name) + " ... " 14 | df = {} 15 | df[run_name] = parse_single_run(sim_data_bag_file) 16 | print "Success." 17 | # Parse plotting config 18 | from analysis_tools.parsing import parse_config_file 19 | pc = parse_config_file("plots.yaml") 20 | print "Success." 21 | return df, pc 22 | 23 | def make_plots(df, pc, run_name, out_dir): 24 | from analysis_tools.plots import desired_vs_actual_for_runs 25 | # Make the Tilt plot 26 | fig = plt.figure(figsize=(20, 10), dpi=40) 27 | ax1 = fig.add_subplot(111) 28 | pc['tilt_control']['runs'] = [run_name] 29 | desired_vs_actual_for_runs(ax1, df, pc['tilt_control']) 30 | fig.tight_layout() 31 | fig_file_name = os.path.join(out_dir, 'TiltControl.png') 32 | plt.savefig(fig_file_name, bbox_inches='tight') 33 | # Make the Velocity plot 34 | fig = plt.figure(figsize=(20, 10), dpi=40) 35 | ax1 = fig.add_subplot(111) 36 | pc['velocity_control']['runs'] = [run_name] 37 | desired_vs_actual_for_runs(ax1, df, pc['velocity_control']) 38 | fig.tight_layout() 39 | fig_file_name = os.path.join(out_dir, 'VelocityControl.png') 40 | plt.savefig(fig_file_name, bbox_inches='tight') 41 | 42 | def main(): 43 | argParser = argparse.ArgumentParser() 44 | default_run = "impulse_force.bag" 45 | argParser.add_argument("-r", "--run", default=default_run, help="Bag file for simulation run of interest") 46 | argParser.add_argument("-o", "--out", default=".", help="Output directory.") 47 | args = argParser.parse_args() 48 | run_name = os.path.splitext(args.run)[0] 49 | df, pc = load_data(args.run, run_name) 50 | # Pass the data and plotting configuration into the make_plots function 51 | make_plots(df, pc, run_name, args.out) 52 | 53 | if __name__ == "__main__": 54 | main() 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /analysis/notebooks/CgPlacement/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys, subprocess 3 | import matplotlib 4 | matplotlib.use('agg') 5 | import matplotlib.pyplot as plt 6 | import seaborn as sns 7 | import pandas as pd 8 | import numpy as np 9 | from analysis_tools.parsing import * 10 | from analysis_tools.plots import * 11 | 12 | # Set some global plotting configs. We want these for all plots 13 | sns.set_context("poster", font_scale=1.1) 14 | #sns.set_palette("Dark2") 15 | 16 | # Set paths to relevant data directories and config files 17 | cgx_data_home = os.path.abspath(os.path.join('data', 'move_cg_x')) 18 | cgx_plot_config = os.path.join(cgx_data_home, 'plots.yaml') 19 | cgz_data_home = os.path.abspath(os.path.join('data', 'move_cg_z')) 20 | cgz_plot_config = os.path.join(cgz_data_home, 'plots.yaml') 21 | 22 | # Load configs and data 23 | pc_x = parse_config_file(cgx_plot_config) 24 | df_x = parse_all_runs_in_dir(cgx_data_home) 25 | pc_z = parse_config_file(cgz_plot_config) 26 | df_z = parse_all_runs_in_dir(cgz_data_home) 27 | -------------------------------------------------------------------------------- /analysis/notebooks/DriveControl/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | os.sys.path.append('/home/mike/.local/lib/python2.7/site-packages/') 4 | import matplotlib 5 | matplotlib.use('agg') 6 | import matplotlib.pyplot as plt 7 | import seaborn as sns 8 | import pandas as pd 9 | import numpy as np 10 | import scipy 11 | import scipy.fftpack 12 | import math 13 | from analysis_tools.parsing import * 14 | from analysis_tools.plots import * 15 | from analysis_tools.filters import * 16 | 17 | # Set some global plotting configs. We want these for all plots 18 | sns.set_context("poster", font_scale=1.1) 19 | #sns.set_palette("Dark2") 20 | 21 | # Set paths to relevant data directories and config files 22 | drive_fwd_bwd_data_home = os.path.abspath(os.path.join('data', 'drive_fwd_bwd')) 23 | drive_square_data_home = os.path.abspath(os.path.join('data', 'drive_square')) 24 | 25 | # Load configs and data 26 | df = parse_all_runs_in_dir(drive_fwd_bwd_data_home) 27 | df.update(parse_all_runs_in_dir(drive_square_data_home)) 28 | 29 | -------------------------------------------------------------------------------- /analysis/notebooks/DriveControl/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['response'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | velocity: 11 | title : 'Velocity - Robot Frame' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 13 | runs : ['response'] 14 | x_label: 'Time (s)' 15 | y_label: 'Velocity (m/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | turn_rate: 20 | title : 'Turn Rate - Robot Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 22 | runs : ['response'] 23 | x_label: 'Time (s)' 24 | y_label: 'Turn Rate (deg/s)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | heading: 29 | title : 'Heading - World Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 31 | runs : ['response'] 32 | x_label: 'Time (s)' 33 | y_label: 'Heading (deg)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'response' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | loop_jitter: 55 | title : 'Loop Jitter' 56 | y_var: 'bobble_bobble_bot_control_node_rt_status__DeltaT' 57 | runs : ['timing'] 58 | x_label: 'Time (s)' 59 | y_label: 'Delta-T (s)' 60 | colors : ['blue'] 61 | xlim: [0.0, 20.0] 62 | 63 | tilt_control: 64 | title : 'Tilt Control' 65 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTilt' 66 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 67 | runs : ['response'] 68 | x_label: 'Time (s)' 69 | y_label: 'Tilt (deg)' 70 | colors : ['red', 'blue'] 71 | legend: 72 | desired: 73 | color: 'red' 74 | linestyle: '-' 75 | actual: 76 | color: 'blue' 77 | linestyle: '-' 78 | xlim: [0.0, 20.0] 79 | 80 | velocity_control: 81 | title : 'Velocity Control' 82 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity' 83 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 84 | runs : ['response'] 85 | x_label: 'Time (s)' 86 | y_label: 'Velocity (m/s)' 87 | cmd_data : 'cmds' 88 | resp_data : 'response' 89 | colors : ['red', 'blue'] 90 | legend: 91 | desired: 92 | color: 'red' 93 | linestyle: '-' 94 | actual: 95 | color: 'blue' 96 | linestyle: '-' 97 | xlim: [0.0, 20.0] 98 | 99 | turning_control: 100 | title : 'Turning Control' 101 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate' 102 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 103 | runs : ['response'] 104 | x_label: 'Time (s)' 105 | y_label: 'Turn Rate (deg/s)' 106 | cmd_data : 'cmds' 107 | resp_data : 'response' 108 | colors : ['red', 'blue'] 109 | legend: 110 | desired: 111 | color: 'red' 112 | linestyle: '-' 113 | actual: 114 | color: 'blue' 115 | linestyle: '-' 116 | xlim: [0.0, 20.0] 117 | 118 | heading_derived: 119 | title : 'Derived Heading - World Frame' 120 | y_var: 'DerivedHeading' 121 | runs : ['response'] 122 | x_label: 'Time (s)' 123 | y_label: 'Heading (deg)' 124 | colors : ['blue'] 125 | xlim: [0.0, 20.0] 126 | 127 | position_derived: 128 | title : 'Derived Position - World Frame' 129 | x_var: 'DerivedXPos' 130 | y_var: 'DerivedYPos' 131 | runs : ['response'] 132 | x_label: 'Pos-X (m)' 133 | y_label: 'Pos-Y (m)' 134 | colors : ['blue'] 135 | xlim: [-1.0, 2.0] 136 | ylim: [-1.0, 2.0] 137 | 138 | heading_control: 139 | title : 'Heading Control - World Frame' 140 | desired_y_var: 'DesiredHeading' 141 | actual_y_var: 'DerivedHeading' 142 | runs : ['response'] 143 | x_label: 'Time (s)' 144 | y_label: 'Heading (deg)' 145 | colors : ['red', 'blue'] 146 | legend: 147 | desired: 148 | color: 'red' 149 | linestyle: '-' 150 | actual: 151 | color: 'blue' 152 | linestyle: '-' 153 | xlim: [0.0, 20.0] 154 | 155 | position_control: 156 | title : 'Position Control - World Frame' 157 | desired_x_var: 'DesiredXPos' 158 | desired_y_var: 'DesiredYPos' 159 | actual_x_var: 'DerivedXPos' 160 | actual_y_var: 'DerivedYPos' 161 | runs : ['response'] 162 | x_label: 'Pos-X (m)' 163 | y_label: 'Pos-Y (m)' 164 | colors : ['red', 'blue'] 165 | legend: 166 | desired: 167 | color: 'red' 168 | linestyle: '-' 169 | actual: 170 | color: 'blue' 171 | linestyle: '-' 172 | xlim: [-1.0, 2.0] 173 | ylim: [-1.0, 2.0] 174 | 175 | position_compare: 176 | title: 'Position - Hw vs Sim' 177 | x_var: 'DerivedXPos' 178 | y_var: 'DerivedYPos' 179 | runs : ['response', 'response_sim'] 180 | x_label: 'Pos-X (m)' 181 | y_label: 'Pos-Y (m)' 182 | colors : ['blue', 'red'] 183 | linestyles: ['-' , '--'] 184 | legend: 185 | hw: 186 | color: 'blue' 187 | linestyle: '-' 188 | sim: 189 | color: 'red' 190 | linestyle: '--' 191 | xlim: [-1.0, 2.0] 192 | ylim: [-1.0, 2.0] -------------------------------------------------------------------------------- /analysis/notebooks/FilterDesign/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | os.sys.path.append('/home/mike/.local/lib/python2.7/site-packages/') 4 | import matplotlib 5 | matplotlib.use('agg') 6 | import matplotlib.pyplot as plt 7 | import seaborn as sns 8 | import pandas as pd 9 | import numpy as np 10 | import scipy 11 | import scipy.fftpack 12 | import math 13 | from analysis_tools.parsing import * 14 | from analysis_tools.plots import * 15 | from analysis_tools.filters import * 16 | 17 | # Set some global plotting configs. We want these for all plots 18 | sns.set_context("poster", font_scale=1.1) 19 | #sns.set_palette("Dark2") 20 | 21 | # Set paths to relevant data directories and config files 22 | gyro_filtering_data_home = os.path.abspath(os.path.join('data', 'gyro_filtering')) 23 | balance_filtering_data_home = os.path.abspath(os.path.join('data', 'balance_filtering')) 24 | 25 | # Load configs and data 26 | df = parse_all_runs_in_dir(gyro_filtering_data_home) 27 | df.update(parse_all_runs_in_dir(balance_filtering_data_home)) 28 | -------------------------------------------------------------------------------- /analysis/notebooks/FilterDesign/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['tilt_testing'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | measured_tilt_rate: 11 | title : 'Tilt Rate' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TiltRate' 13 | runs : ['tilt_testing'] 14 | x_label: 'Time (s)' 15 | y_label: 'Tilt Rate (deg/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | heading: 20 | title : 'Heading - World Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 22 | runs : ['turn_testing'] 23 | x_label: 'Time (s)' 24 | y_label: 'Heading (deg)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | turn_rate: 29 | title : 'Turn Rate - Robot Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 31 | runs : ['turn_testing'] 32 | x_label: 'Time (s)' 33 | y_label: 'Turn Rate (deg/s)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'turn_testing' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | lp_filter_freq_response: 55 | raw_signal: 'bobble_bobble_balance_controller_bb_controller_status__MeasuredTurnRate' 56 | run : 'turn_testing' 57 | sample_rate: 200.0 58 | #cutoff_freq: 20.0 59 | cutoff_freq: 5.0 60 | filter_order: 3 61 | runs : ['turn_testing'] 62 | y_label: 'Turn Rate (deg/s)' 63 | colors : ['blue'] 64 | xlim: [0.0, 20.0] 65 | -------------------------------------------------------------------------------- /analysis/notebooks/RosJupyterAnalysis/RosJupyterAnalysisPart1.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "\n", 8 | "
\n", 9 | " \"Jupyter\"\n", 10 | "
\n", 11 | "\n", 12 | "[Jupyter Notebook](https://jupyter.org/) is a commonly used open-source \n", 13 | "tool in data science. It allows for organizing and sharing data processing \n", 14 | "and analysis code in a readable notebook style format. There are many \n", 15 | "[great examples](https://github.com/jupyter/jupyter/wiki/A-gallery-of-interesting-Jupyter-Notebooks) \n", 16 | "of Jupyter Notebook on the web. This blog post is part one of a three part series aimed at \n", 17 | "showing how this powerful tool can be used to analyze ROS data. Hopefully it \n", 18 | "will serve as a complete example of how to create your own notebook to \n", 19 | "analyze your robot's data. In part one we will use ROS and Gazebo to generate \n", 20 | "some simulation data that we will analyze in parts two and three." 21 | ] 22 | }, 23 | { 24 | "cell_type": "markdown", 25 | "metadata": {}, 26 | "source": [ 27 | "## Getting Some Sample Robot Data\n", 28 | "Before we begin our analysis of ROS data with Jupyter, we need some robot data. \n", 29 | "To get this data, we will use the simulator for the open-source robot, \n", 30 | "Bobble-Bot. This project contains a high fidelity simulation of a two \n", 31 | "wheeled self-balancing robot. The simulator has been validated against \n", 32 | "test runs of the real robot created by the engineers at [SOE](https://so.engineering). \n", 33 | "For more information, see the project's\n", 34 | "[GitHub page](https://github.com/super-owesome/bobble_controllers). By the end of this \n", 35 | "post, we will have grabbed data from some simulation runs like the one shown below.\n", 36 | "\n", 37 | "![BobbleSim](https://media.giphy.com/media/5QPazHiJQLuBGeiu5z/giphy.gif \"Bobble-Bot Sim\")\n", 38 | "\n", 39 | "If you would like to focus on the ROS/Jupyter analysis aspect, you can skip the \n", 40 | "simulation steps below and proceed to download the ROS simulation data from [here](https://github.com/super-owesome/bobble_controllers/raw/master/analysis/notebooks/RosJupyterAnalysis/data/sample_data.zip) and jump ahead to [part 2]({static}/ros-analysis-part-2)." 41 | ] 42 | }, 43 | { 44 | "cell_type": "markdown", 45 | "metadata": {}, 46 | "source": [ 47 | "## Building and running the simulation\n", 48 | "The Bobble-Bot simulator requires ROS and Gazebo. Follow the instructions \n", 49 | "[here](http://wiki.ros.org/melodic/Installation/Ubuntu)\n", 50 | "and install ROS Melodic Desktop. Other recent versions of ROS should also work, \n", 51 | "but they are not officially supported at this time. The simulator also makes \n", 52 | "use of the [Hector Gazebo](http://wiki.ros.org/hector_gazebo_plugins) \n", 53 | "plugins. Those can be installed using the command below.\n", 54 | "\n", 55 | "```bash\n", 56 | "apt-get install ros-melodic-hector-gazebo-plugins\n", 57 | "```\n", 58 | "\n", 59 | "Before starting the build process, make sure your ROS environment is active.\n", 60 | "\n", 61 | "```bash\n", 62 | "source /opt/ros/melodic/setup.bash\n", 63 | "```\n", 64 | "\n", 65 | "Get the simulation code and build it using catkin.\n", 66 | "\n", 67 | "```bash\n", 68 | "mkdir -p ~/bobble_workspace/src\n", 69 | "cd ~/bobble_workspace/src\n", 70 | "catkin_init_workspace\n", 71 | "git clone https://github.com/super-owesome/bobble_controllers.git\n", 72 | "git clone https://github.com/super-owesome/bobble_description.git\n", 73 | "```\n", 74 | "By now your bobble_workspace directory should look like so:\n", 75 | "\n", 76 | "```text\n", 77 | "└── src\n", 78 | " ├── bobble_controllers\n", 79 | " │   ├── analysis\n", 80 | " │   ├── config\n", 81 | " │   ├── launch\n", 82 | " │   ├── src\n", 83 | " | ...\n", 84 | " ├── bobble_description\n", 85 | " │   ├── imgs\n", 86 | " │   ├── launch\n", 87 | " │   ├── meshes\n", 88 | " │   └── xacro\n", 89 | " | ...\n", 90 | " └── CMakeLists.txt\n", 91 | "```\n", 92 | "\n", 93 | "Build the Bobble-Bot simulation by running this command:\n", 94 | "\n", 95 | "```bash\n", 96 | "catkin_make\n", 97 | "catkin_make install\n", 98 | "source install/setup.bash\n", 99 | "```\n", 100 | "\n", 101 | "The Bobble-Bot controller package comes with a set of launch files that can \n", 102 | "be used to generate data from different runs of the simulation. As an \n", 103 | "example, the apply_impulse_force.launch file can be used to run the Bobble-Bot \n", 104 | "simulation and apply an impulse force in the X direction. This launch file accepts \n", 105 | "a few launch parameters. \n", 106 | "Most notably, the X component of the impulse force, and a flag to enable/disable \n", 107 | "the 3D graphics. \n", 108 | "\n", 109 | "```bash\n", 110 | "roslaunch bobble_controllers apply_impulse_force.launch \\\n", 111 | "impulse_force:=-1000 gui:=true\n", 112 | "```\n", 113 | "\n", 114 | "The launch file also instructs the simulation to log simulation data during the run.\n", 115 | "The data is placed in the following location by default:\n", 116 | "\n", 117 | "```bash\n", 118 | "ls ~/.ros | grep impulse_data\n", 119 | "impulse_data.bag\n", 120 | "```\n", 121 | "\n", 122 | "You can override this default location like so:\n", 123 | "\n", 124 | "```bash\n", 125 | "roslaunch bobble_controllers apply_impulse_force.launch \\\n", 126 | "impulse_force:=-1000 gui:=true out_file:=/path/to/file/name_no_ext\n", 127 | "```" 128 | ] 129 | }, 130 | { 131 | "cell_type": "markdown", 132 | "metadata": {}, 133 | "source": [ 134 | "## Generating some more data\n", 135 | "The bobble_controllers package comes with a set of automated tests that can be used \n", 136 | "to quickly and easily generate data from four different test runs of the \n", 137 | "Bobble-Bot balance controller. These tests can be run using the following \n", 138 | "commands.\n", 139 | "\n", 140 | "```bash\n", 141 | "cd ~/bobble_workspace\n", 142 | "catkin run_tests -j1\n", 143 | "```\n", 144 | "\n", 145 | "These tests will take a few minutes to run. A lot is happening behind the scenes \n", 146 | "and your data is being generated. Now would be a good time to grab a cup \n", 147 | "of coffee. Once the tests have completed you can get a summary of the results using the \n", 148 | "commands below:\n", 149 | "\n", 150 | "```bash\n", 151 | "catkin_test_results --verbose .\n", 152 | "Summary: 18 tests, 0 errors, 0 failures, 0 skipped\n", 153 | "```\n", 154 | "\n", 155 | "If all went well, your tests will have all passed. If you encounter a failure, \n", 156 | "post a comment below. The tests generated simulation results from four different \n", 157 | "runs and recorded the data from each into ROS bag files. The table below briefly \n", 158 | "summarizes each test that was performed.\n", 159 | "\n", 160 | "\n", 161 | "| Test Name | Description | Source Code |\n", 162 | "| ------------------- | -------------------------------------------- | -------------- |\n", 163 | "| No Balance | Bobble-Bot uncontrolled | [no_balance.test](https://github.com/super-owesome/bobble_controllers/blob/master/test/no_balance.test) |\n", 164 | "| Impulse force | Balance control active, impulse applied. | [impulse_force](https://github.com/super-owesome/bobble_controllers/blob/master/test/impulse_force.test) |\n", 165 | "| Balance Control | Balance control active, drive around. | [balance_control](https://github.com/super-owesome/bobble_controllers/blob/master/test/balance_controller.test) |\n", 166 | "| Drive square | Balance control active, drive in square. | [drive_square](https://github.com/super-owesome/bobble_controllers/blob/master/test/js_drive_square.test) |\n", 167 | "\n", 168 | "\n", 169 | "The data can be found in the following directory:\n", 170 | "\n", 171 | "```text\n", 172 | "src/bobble_controllers/\n", 173 | "└── test\n", 174 | " ├── data\n", 175 | " │   ├── balance.bag\n", 176 | " │   ├── balance.csv\n", 177 | " │   ├── drive_square_js_response.bag\n", 178 | " │   ├── drive_square_js_response.csv\n", 179 | " │   ├── impulse_force.bag\n", 180 | " │   ├── impulse_force.csv\n", 181 | " │   ├── no_balance.bag\n", 182 | " │   └── no_balance.csv\n", 183 | "```\n", 184 | "\n", 185 | "If anything went wrong, you can simply download the data [here](https://github.com/super-owesome/bobble_controllers/raw/master/analysis/notebooks/RosJupyterAnalysis/data/sample_data.zip).\n", 186 | "Now that we have our data, we can move on to part two of this series where we will \n", 187 | "set up Jupyter Notebook to begin the analysis." 188 | ] 189 | } 190 | ], 191 | "metadata": { 192 | "kernelspec": { 193 | "display_name": "Python 2", 194 | "language": "python", 195 | "name": "python2" 196 | }, 197 | "language_info": { 198 | "codemirror_mode": { 199 | "name": "ipython", 200 | "version": 2 201 | }, 202 | "file_extension": ".py", 203 | "mimetype": "text/x-python", 204 | "name": "python", 205 | "nbconvert_exporter": "python", 206 | "pygments_lexer": "ipython2", 207 | "version": "2.7.15rc1" 208 | }, 209 | "toc": { 210 | "base_numbering": 1, 211 | "nav_menu": {}, 212 | "number_sections": true, 213 | "sideBar": true, 214 | "skip_h1_title": false, 215 | "title_cell": "Table of Contents", 216 | "title_sidebar": "Contents", 217 | "toc_cell": false, 218 | "toc_position": {}, 219 | "toc_section_display": true, 220 | "toc_window_display": false 221 | } 222 | }, 223 | "nbformat": 4, 224 | "nbformat_minor": 2 225 | } 226 | -------------------------------------------------------------------------------- /analysis/notebooks/RosJupyterAnalysis/data/sample_data.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/analysis/notebooks/RosJupyterAnalysis/data/sample_data.zip -------------------------------------------------------------------------------- /analysis/notebooks/RosJupyterAnalysis/nb_env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | import matplotlib 4 | matplotlib.use('agg') 5 | import matplotlib.pyplot as plt 6 | import warnings 7 | warnings.filterwarnings("ignore") 8 | import seaborn as sns 9 | import pandas as pd 10 | import numpy as np 11 | import scipy 12 | import scipy.fftpack 13 | import math 14 | try: 15 | from analysis_tools.parsing import * 16 | from analysis_tools.plots import * 17 | from analysis_tools.filters import * 18 | except ImportError: 19 | sys.path.append(os.path.join('..', '..', '..', 'src')) 20 | from analysis_tools.parsing import * 21 | from analysis_tools.plots import * 22 | from analysis_tools.filters import * 23 | 24 | # Set some global plotting configs. We want these for all plots 25 | sns.set_context("poster", font_scale=1.1) 26 | 27 | # Set paths to relevant data directories and config files 28 | data_home = os.path.join(os.getcwd(), "data") 29 | print("Reading all bag files in directory : ") 30 | print(data_home) 31 | 32 | # Load configs and data 33 | df = parse_all_runs_in_dir(data_home) 34 | print("Successfully loaded runs : ") 35 | 36 | -------------------------------------------------------------------------------- /analysis/notebooks/RosJupyterAnalysis/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'Tilt' 4 | x_label: 'Time (s)' 5 | y_label: 'Tilt (deg)' 6 | runs : ['no_balance', 'impulse_force'] 7 | colors : ['grey', 'orange'] 8 | linestyles: ['-' , '-'] 9 | legend: 10 | no balance: 11 | color: 'grey' 12 | linestyle: '-' 13 | balanced: 14 | color: 'orange' 15 | linestyle: '-' 16 | xlim: [0.0, 10.0] 17 | 18 | velocity: 19 | title : 'Velocity' 20 | y_var: 'ForwardVelocity' 21 | x_label: 'Time (s)' 22 | y_label: 'Velocity (m/s)' 23 | runs : ['no_balance', 'impulse_force'] 24 | colors : ['grey', 'orange'] 25 | linestyles: ['-' , '-'] 26 | legend: 27 | no balance: 28 | color: 'grey' 29 | linestyle: '-' 30 | balanced: 31 | color: 'orange' 32 | linestyle: '-' 33 | xlim: [0.0, 10.0] 34 | 35 | tilt_control: 36 | title : 'Tilt Control' 37 | desired_y_var: 'DesiredTilt' 38 | actual_y_var: 'Tilt' 39 | runs : ['balance'] 40 | x_label: 'Time (s)' 41 | y_label: 'Tilt (deg)' 42 | colors : ['red', 'blue'] 43 | legend: 44 | desired: 45 | color: 'red' 46 | linestyle: '-' 47 | actual: 48 | color: 'blue' 49 | linestyle: '-' 50 | xlim: [0.0, 10.0] 51 | 52 | velocity_control: 53 | title : 'Velocity Control' 54 | desired_y_var: 'DesiredVelocity' 55 | actual_y_var: 'ForwardVelocity' 56 | runs : ['balance'] 57 | x_label: 'Time (s)' 58 | y_label: 'Velocity (m/s)' 59 | colors : ['red', 'blue'] 60 | legend: 61 | desired: 62 | color: 'red' 63 | linestyle: '-' 64 | actual: 65 | color: 'blue' 66 | linestyle: '-' 67 | xlim: [0.0, 10.0] 68 | 69 | turn_rate: 70 | title : 'Turn Rate - Robot Frame' 71 | y_var: 'TurnRate' 72 | runs : ['balance'] 73 | x_label: 'Time (s)' 74 | y_label: 'Turn Rate (deg/s)' 75 | colors : ['blue'] 76 | xlim: [0.0, 30.0] 77 | 78 | heading: 79 | title : 'Heading - World Frame' 80 | y_var: 'Heading' 81 | runs : ['balance'] 82 | x_label: 'Time (s)' 83 | y_label: 'Heading (deg)' 84 | colors : ['blue'] 85 | xlim: [0.0, 30.0] 86 | 87 | wheel_vel: 88 | title : 'Wheel Velocity' 89 | vars: ['LeftMotorVelocity', 90 | 'RightMotorVelocity'] 91 | runs : ['balance'] 92 | x_label: 'Time (s)' 93 | y_label: 'Velocity (deg/s)' 94 | colors : ['orange', 'grey'] 95 | legend: 96 | left: 97 | color: 'orange' 98 | linestyle: '-' 99 | right: 100 | color: 'grey' 101 | linestyle: '-' 102 | xlim: [0.0, 30.0] 103 | 104 | loop_jitter: 105 | title : 'Loop Jitter' 106 | y_var: 'DeltaT' 107 | runs : ['balance'] 108 | x_label: 'Time (s)' 109 | y_label: 'Delta-T (s)' 110 | colors : ['blue'] 111 | xlim: [0.0, 30.0] 112 | 113 | turning_control: 114 | title : 'Turning Control' 115 | desired_y_var: 'DesiredTurnRate' 116 | actual_y_var: 'TurnRate' 117 | runs : ['balance'] 118 | x_label: 'Time (s)' 119 | y_label: 'Turn Rate (deg/s)' 120 | colors : ['red', 'blue'] 121 | legend: 122 | desired: 123 | color: 'red' 124 | linestyle: '-' 125 | actual: 126 | color: 'blue' 127 | linestyle: '-' 128 | xlim: [0.0, 30.0] 129 | 130 | heading_derived: 131 | title : 'Derived Heading - World Frame' 132 | y_var: 'DerivedHeading' 133 | runs : ['balance'] 134 | x_label: 'Time (s)' 135 | y_label: 'Heading (deg)' 136 | colors : ['blue'] 137 | xlim: [0.0, 30.0] 138 | 139 | position_derived: 140 | title : 'Derived Position - World Frame' 141 | x_var: 'DerivedXPos' 142 | y_var: 'DerivedYPos' 143 | runs : ['balance'] 144 | x_label: 'Pos-X (m)' 145 | y_label: 'Pos-Y (m)' 146 | colors : ['blue'] 147 | xlim: [-1.0, 2.0] 148 | ylim: [-1.0, 2.0] 149 | 150 | heading_control: 151 | title : 'Heading Control - World Frame' 152 | desired_y_var: 'DesiredHeading' 153 | actual_y_var: 'DerivedHeading' 154 | runs : ['balance'] 155 | x_label: 'Time (s)' 156 | y_label: 'Heading (deg)' 157 | colors : ['red', 'blue'] 158 | legend: 159 | desired: 160 | color: 'red' 161 | linestyle: '-' 162 | actual: 163 | color: 'blue' 164 | linestyle: '-' 165 | xlim: [0.0, 30.0] 166 | 167 | position_control: 168 | title : 'Position Control - World Frame' 169 | desired_x_var: 'DesiredXPos' 170 | desired_y_var: 'DesiredYPos' 171 | actual_x_var: 'DerivedXPos' 172 | actual_y_var: 'DerivedYPos' 173 | runs : ['balance'] 174 | x_label: 'Pos-X (m)' 175 | y_label: 'Pos-Y (m)' 176 | colors : ['red', 'blue'] 177 | legend: 178 | desired: 179 | color: 'red' 180 | linestyle: '-' 181 | actual: 182 | color: 'blue' 183 | linestyle: '-' 184 | xlim: [-1.0, 2.0] 185 | ylim: [-1.0, 2.0] 186 | 187 | -------------------------------------------------------------------------------- /analysis/notebooks/TiltControl/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | os.sys.path.append('/home/mike/.local/lib/python2.7/site-packages/') 4 | import matplotlib 5 | matplotlib.use('agg') 6 | import matplotlib.pyplot as plt 7 | import seaborn as sns 8 | import pandas as pd 9 | import numpy as np 10 | import scipy 11 | import scipy.fftpack 12 | import math 13 | from analysis_tools.parsing import * 14 | from analysis_tools.plots import * 15 | from analysis_tools.filters import * 16 | 17 | # Set some global plotting configs. We want these for all plots 18 | sns.set_context("poster", font_scale=1.1) 19 | #sns.set_palette("Dark2") 20 | 21 | # Set paths to relevant data directories and config files 22 | tilt_tests_data_home = os.path.abspath(os.path.join('data', 'tilt_tests')) 23 | 24 | # Load configs and data 25 | df = parse_all_runs_in_dir(tilt_tests_data_home) 26 | #df.update(parse_all_runs_in_dir(drive_square_data_home)) 27 | 28 | -------------------------------------------------------------------------------- /analysis/notebooks/TiltControl/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['response'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | velocity: 11 | title : 'Velocity - Robot Frame' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 13 | runs : ['response'] 14 | x_label: 'Time (s)' 15 | y_label: 'Velocity (m/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | turn_rate: 20 | title : 'Turn Rate - Robot Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 22 | runs : ['response'] 23 | x_label: 'Time (s)' 24 | y_label: 'Turn Rate (deg/s)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | heading: 29 | title : 'Heading - World Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 31 | runs : ['response'] 32 | x_label: 'Time (s)' 33 | y_label: 'Heading (deg)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'response' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | loop_jitter: 55 | title : 'Loop Jitter' 56 | y_var: 'bobble_bobble_bot_control_node_rt_status__DeltaT' 57 | runs : ['timing'] 58 | x_label: 'Time (s)' 59 | y_label: 'Delta-T (s)' 60 | colors : ['blue'] 61 | xlim: [0.0, 20.0] 62 | 63 | tilt_control: 64 | title : 'Tilt Control' 65 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTilt' 66 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 67 | runs : ['response'] 68 | x_label: 'Time (s)' 69 | y_label: 'Tilt (deg)' 70 | colors : ['red', 'blue'] 71 | legend: 72 | desired: 73 | color: 'red' 74 | linestyle: '-' 75 | actual: 76 | color: 'blue' 77 | linestyle: '-' 78 | xlim: [0.0, 20.0] 79 | 80 | velocity_control: 81 | title : 'Velocity Control' 82 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity' 83 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 84 | runs : ['response'] 85 | x_label: 'Time (s)' 86 | y_label: 'Velocity (m/s)' 87 | cmd_data : 'cmds' 88 | resp_data : 'response' 89 | colors : ['red', 'blue'] 90 | legend: 91 | desired: 92 | color: 'red' 93 | linestyle: '-' 94 | actual: 95 | color: 'blue' 96 | linestyle: '-' 97 | xlim: [0.0, 20.0] 98 | 99 | turning_control: 100 | title : 'Turning Control' 101 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate' 102 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 103 | runs : ['response'] 104 | x_label: 'Time (s)' 105 | y_label: 'Turn Rate (deg/s)' 106 | cmd_data : 'cmds' 107 | resp_data : 'response' 108 | colors : ['red', 'blue'] 109 | legend: 110 | desired: 111 | color: 'red' 112 | linestyle: '-' 113 | actual: 114 | color: 'blue' 115 | linestyle: '-' 116 | xlim: [0.0, 20.0] 117 | 118 | heading_derived: 119 | title : 'Derived Heading - World Frame' 120 | y_var: 'DerivedHeading' 121 | runs : ['response'] 122 | x_label: 'Time (s)' 123 | y_label: 'Heading (deg)' 124 | colors : ['blue'] 125 | xlim: [0.0, 20.0] 126 | 127 | position_derived: 128 | title : 'Derived Position - World Frame' 129 | x_var: 'DerivedXPos' 130 | y_var: 'DerivedYPos' 131 | runs : ['response'] 132 | x_label: 'Pos-X (m)' 133 | y_label: 'Pos-Y (m)' 134 | colors : ['blue'] 135 | xlim: [-1.0, 2.0] 136 | ylim: [-1.0, 2.0] 137 | 138 | heading_control: 139 | title : 'Heading Control - World Frame' 140 | desired_y_var: 'DesiredHeading' 141 | actual_y_var: 'DerivedHeading' 142 | runs : ['response'] 143 | x_label: 'Time (s)' 144 | y_label: 'Heading (deg)' 145 | colors : ['red', 'blue'] 146 | legend: 147 | desired: 148 | color: 'red' 149 | linestyle: '-' 150 | actual: 151 | color: 'blue' 152 | linestyle: '-' 153 | xlim: [0.0, 20.0] 154 | 155 | position_control: 156 | title : 'Position Control - World Frame' 157 | desired_x_var: 'DesiredXPos' 158 | desired_y_var: 'DesiredYPos' 159 | actual_x_var: 'DerivedXPos' 160 | actual_y_var: 'DerivedYPos' 161 | runs : ['response'] 162 | x_label: 'Pos-X (m)' 163 | y_label: 'Pos-Y (m)' 164 | colors : ['red', 'blue'] 165 | legend: 166 | desired: 167 | color: 'red' 168 | linestyle: '-' 169 | actual: 170 | color: 'blue' 171 | linestyle: '-' 172 | xlim: [-1.0, 2.0] 173 | ylim: [-1.0, 2.0] 174 | 175 | position_compare: 176 | title: 'Position - Hw vs Sim' 177 | x_var: 'DerivedXPos' 178 | y_var: 'DerivedYPos' 179 | runs : ['response', 'response_sim'] 180 | x_label: 'Pos-X (m)' 181 | y_label: 'Pos-Y (m)' 182 | colors : ['blue', 'red'] 183 | linestyles: ['-' , '--'] 184 | legend: 185 | hw: 186 | color: 'blue' 187 | linestyle: '-' 188 | sim: 189 | color: 'red' 190 | linestyle: '--' 191 | xlim: [-1.0, 2.0] 192 | ylim: [-1.0, 2.0] -------------------------------------------------------------------------------- /analysis/notebooks/TurningControl/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | os.sys.path.append('/home/mike/.local/lib/python2.7/site-packages/') 4 | import matplotlib 5 | matplotlib.use('agg') 6 | import matplotlib.pyplot as plt 7 | import seaborn as sns 8 | import pandas as pd 9 | import numpy as np 10 | import scipy 11 | import scipy.fftpack 12 | import math 13 | from analysis_tools.parsing import * 14 | from analysis_tools.plots import * 15 | from analysis_tools.filters import * 16 | 17 | # Set some global plotting configs. We want these for all plots 18 | sns.set_context("poster", font_scale=1.1) 19 | #sns.set_palette("Dark2") 20 | 21 | # Set paths to relevant data directories and config files 22 | turning_tests_data_home = os.path.abspath(os.path.join('data', 'turning_tests')) 23 | 24 | # Load configs and data 25 | df = parse_all_runs_in_dir(turning_tests_data_home) 26 | #df.update(parse_all_runs_in_dir(drive_square_data_home)) 27 | 28 | -------------------------------------------------------------------------------- /analysis/notebooks/TurningControl/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['response'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | velocity: 11 | title : 'Velocity - Robot Frame' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 13 | runs : ['response'] 14 | x_label: 'Time (s)' 15 | y_label: 'Velocity (m/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | turn_rate: 20 | title : 'Turn Rate - Robot Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 22 | runs : ['response'] 23 | x_label: 'Time (s)' 24 | y_label: 'Turn Rate (deg/s)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | heading: 29 | title : 'Heading - World Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 31 | runs : ['response'] 32 | x_label: 'Time (s)' 33 | y_label: 'Heading (deg)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'response' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | loop_jitter: 55 | title : 'Loop Jitter' 56 | y_var: 'bobble_bobble_bot_control_node_rt_status__DeltaT' 57 | runs : ['timing'] 58 | x_label: 'Time (s)' 59 | y_label: 'Delta-T (s)' 60 | colors : ['blue'] 61 | xlim: [0.0, 20.0] 62 | 63 | tilt_control: 64 | title : 'Tilt Control' 65 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTilt' 66 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 67 | runs : ['response'] 68 | x_label: 'Time (s)' 69 | y_label: 'Tilt (deg)' 70 | colors : ['red', 'blue'] 71 | legend: 72 | desired: 73 | color: 'red' 74 | linestyle: '-' 75 | actual: 76 | color: 'blue' 77 | linestyle: '-' 78 | xlim: [0.0, 20.0] 79 | 80 | velocity_control: 81 | title : 'Velocity Control' 82 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity' 83 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 84 | runs : ['response'] 85 | x_label: 'Time (s)' 86 | y_label: 'Velocity (m/s)' 87 | cmd_data : 'cmds' 88 | resp_data : 'response' 89 | colors : ['red', 'blue'] 90 | legend: 91 | desired: 92 | color: 'red' 93 | linestyle: '-' 94 | actual: 95 | color: 'blue' 96 | linestyle: '-' 97 | xlim: [0.0, 20.0] 98 | 99 | turning_control: 100 | title : 'Turning Control' 101 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate' 102 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 103 | runs : ['response'] 104 | x_label: 'Time (s)' 105 | y_label: 'Turn Rate (deg/s)' 106 | cmd_data : 'cmds' 107 | resp_data : 'response' 108 | colors : ['red', 'blue'] 109 | legend: 110 | desired: 111 | color: 'red' 112 | linestyle: '-' 113 | actual: 114 | color: 'blue' 115 | linestyle: '-' 116 | xlim: [0.0, 20.0] 117 | 118 | heading_derived: 119 | title : 'Derived Heading - World Frame' 120 | y_var: 'DerivedHeading' 121 | runs : ['response'] 122 | x_label: 'Time (s)' 123 | y_label: 'Heading (deg)' 124 | colors : ['blue'] 125 | xlim: [0.0, 20.0] 126 | 127 | position_derived: 128 | title : 'Derived Position - World Frame' 129 | x_var: 'DerivedXPos' 130 | y_var: 'DerivedYPos' 131 | runs : ['response'] 132 | x_label: 'Pos-X (m)' 133 | y_label: 'Pos-Y (m)' 134 | colors : ['blue'] 135 | xlim: [-1.0, 2.0] 136 | ylim: [-1.0, 2.0] 137 | 138 | heading_control: 139 | title : 'Heading Control - World Frame' 140 | desired_y_var: 'DesiredHeading' 141 | actual_y_var: 'DerivedHeading' 142 | runs : ['response'] 143 | x_label: 'Time (s)' 144 | y_label: 'Heading (deg)' 145 | colors : ['red', 'blue'] 146 | legend: 147 | desired: 148 | color: 'red' 149 | linestyle: '-' 150 | actual: 151 | color: 'blue' 152 | linestyle: '-' 153 | xlim: [0.0, 20.0] 154 | 155 | position_control: 156 | title : 'Position Control - World Frame' 157 | desired_x_var: 'DesiredXPos' 158 | desired_y_var: 'DesiredYPos' 159 | actual_x_var: 'DerivedXPos' 160 | actual_y_var: 'DerivedYPos' 161 | runs : ['response'] 162 | x_label: 'Pos-X (m)' 163 | y_label: 'Pos-Y (m)' 164 | colors : ['red', 'blue'] 165 | legend: 166 | desired: 167 | color: 'red' 168 | linestyle: '-' 169 | actual: 170 | color: 'blue' 171 | linestyle: '-' 172 | xlim: [-1.0, 2.0] 173 | ylim: [-1.0, 2.0] 174 | 175 | position_compare: 176 | title: 'Position - Hw vs Sim' 177 | x_var: 'DerivedXPos' 178 | y_var: 'DerivedYPos' 179 | runs : ['response', 'response_sim'] 180 | x_label: 'Pos-X (m)' 181 | y_label: 'Pos-Y (m)' 182 | colors : ['blue', 'red'] 183 | linestyles: ['-' , '--'] 184 | legend: 185 | hw: 186 | color: 'blue' 187 | linestyle: '-' 188 | sim: 189 | color: 'red' 190 | linestyle: '--' 191 | xlim: [-1.0, 2.0] 192 | ylim: [-1.0, 2.0] -------------------------------------------------------------------------------- /analysis/notebooks/VelocityControl/env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | os.sys.path.append('/home/mike/.local/lib/python2.7/site-packages/') 4 | import matplotlib 5 | matplotlib.use('agg') 6 | import matplotlib.pyplot as plt 7 | import seaborn as sns 8 | import pandas as pd 9 | import numpy as np 10 | import scipy 11 | import scipy.fftpack 12 | import math 13 | from analysis_tools.parsing import * 14 | from analysis_tools.plots import * 15 | from analysis_tools.filters import * 16 | 17 | # Set some global plotting configs. We want these for all plots 18 | sns.set_context("poster", font_scale=1.1) 19 | #sns.set_palette("Dark2") 20 | 21 | # Set paths to relevant data directories and config files 22 | velocity_tests_data_home = os.path.abspath(os.path.join('data', 'velocity_tests')) 23 | 24 | # Load configs and data 25 | df = parse_all_runs_in_dir(velocity_tests_data_home) 26 | #df.update(parse_all_runs_in_dir(drive_square_data_home)) 27 | 28 | -------------------------------------------------------------------------------- /analysis/notebooks/VelocityControl/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['response'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | velocity: 11 | title : 'Velocity - Robot Frame' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 13 | runs : ['response'] 14 | x_label: 'Time (s)' 15 | y_label: 'Velocity (m/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | turn_rate: 20 | title : 'Turn Rate - Robot Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 22 | runs : ['response'] 23 | x_label: 'Time (s)' 24 | y_label: 'Turn Rate (deg/s)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | heading: 29 | title : 'Heading - World Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 31 | runs : ['response'] 32 | x_label: 'Time (s)' 33 | y_label: 'Heading (deg)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'response' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | loop_jitter: 55 | title : 'Loop Jitter' 56 | y_var: 'bobble_bobble_bot_control_node_rt_status__DeltaT' 57 | runs : ['timing'] 58 | x_label: 'Time (s)' 59 | y_label: 'Delta-T (s)' 60 | colors : ['blue'] 61 | xlim: [0.0, 20.0] 62 | 63 | tilt_control: 64 | title : 'Tilt Control' 65 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTilt' 66 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 67 | runs : ['response'] 68 | x_label: 'Time (s)' 69 | y_label: 'Tilt (deg)' 70 | colors : ['red', 'blue'] 71 | legend: 72 | desired: 73 | color: 'red' 74 | linestyle: '-' 75 | actual: 76 | color: 'blue' 77 | linestyle: '-' 78 | xlim: [0.0, 20.0] 79 | 80 | velocity_control: 81 | title : 'Velocity Control' 82 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity' 83 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 84 | runs : ['response'] 85 | x_label: 'Time (s)' 86 | y_label: 'Velocity (m/s)' 87 | cmd_data : 'cmds' 88 | resp_data : 'response' 89 | colors : ['red', 'blue'] 90 | legend: 91 | desired: 92 | color: 'red' 93 | linestyle: '-' 94 | actual: 95 | color: 'blue' 96 | linestyle: '-' 97 | xlim: [0.0, 20.0] 98 | 99 | turning_control: 100 | title : 'Turning Control' 101 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate' 102 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 103 | runs : ['response'] 104 | x_label: 'Time (s)' 105 | y_label: 'Turn Rate (deg/s)' 106 | cmd_data : 'cmds' 107 | resp_data : 'response' 108 | colors : ['red', 'blue'] 109 | legend: 110 | desired: 111 | color: 'red' 112 | linestyle: '-' 113 | actual: 114 | color: 'blue' 115 | linestyle: '-' 116 | xlim: [0.0, 20.0] 117 | 118 | heading_derived: 119 | title : 'Derived Heading - World Frame' 120 | y_var: 'DerivedHeading' 121 | runs : ['response'] 122 | x_label: 'Time (s)' 123 | y_label: 'Heading (deg)' 124 | colors : ['blue'] 125 | xlim: [0.0, 20.0] 126 | 127 | position_derived: 128 | title : 'Derived Position - World Frame' 129 | x_var: 'DerivedXPos' 130 | y_var: 'DerivedYPos' 131 | runs : ['response'] 132 | x_label: 'Pos-X (m)' 133 | y_label: 'Pos-Y (m)' 134 | colors : ['blue'] 135 | xlim: [-1.0, 2.0] 136 | ylim: [-1.0, 2.0] 137 | 138 | heading_control: 139 | title : 'Heading Control - World Frame' 140 | desired_y_var: 'DesiredHeading' 141 | actual_y_var: 'DerivedHeading' 142 | runs : ['response'] 143 | x_label: 'Time (s)' 144 | y_label: 'Heading (deg)' 145 | colors : ['red', 'blue'] 146 | legend: 147 | desired: 148 | color: 'red' 149 | linestyle: '-' 150 | actual: 151 | color: 'blue' 152 | linestyle: '-' 153 | xlim: [0.0, 20.0] 154 | 155 | position_control: 156 | title : 'Position Control - World Frame' 157 | desired_x_var: 'DesiredXPos' 158 | desired_y_var: 'DesiredYPos' 159 | actual_x_var: 'DerivedXPos' 160 | actual_y_var: 'DerivedYPos' 161 | runs : ['response'] 162 | x_label: 'Pos-X (m)' 163 | y_label: 'Pos-Y (m)' 164 | colors : ['red', 'blue'] 165 | legend: 166 | desired: 167 | color: 'red' 168 | linestyle: '-' 169 | actual: 170 | color: 'blue' 171 | linestyle: '-' 172 | xlim: [-1.0, 2.0] 173 | ylim: [-1.0, 2.0] 174 | 175 | position_compare: 176 | title: 'Position - Hw vs Sim' 177 | x_var: 'DerivedXPos' 178 | y_var: 'DerivedYPos' 179 | runs : ['response', 'response_sim'] 180 | x_label: 'Pos-X (m)' 181 | y_label: 'Pos-Y (m)' 182 | colors : ['blue', 'red'] 183 | linestyles: ['-' , '--'] 184 | legend: 185 | hw: 186 | color: 'blue' 187 | linestyle: '-' 188 | sim: 189 | color: 'red' 190 | linestyle: '--' 191 | xlim: [-1.0, 2.0] 192 | ylim: [-1.0, 2.0] -------------------------------------------------------------------------------- /analysis/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'Tilt' 4 | x_label: 'Time (s)' 5 | y_label: 'Tilt (deg)' 6 | runs : ['impulse_force'] 7 | colors : ['grey', 'orange'] 8 | linestyles: ['-' , '-'] 9 | legend: 10 | no balance: 11 | color: 'grey' 12 | linestyle: '-' 13 | balanced: 14 | color: 'orange' 15 | linestyle: '-' 16 | xlim: [0.0, 10.0] 17 | 18 | velocity: 19 | title : 'Velocity' 20 | y_var: 'ForwardVelocity' 21 | x_label: 'Time (s)' 22 | y_label: 'Velocity (m/s)' 23 | runs : ['impulse_force'] 24 | colors : ['grey', 'orange'] 25 | linestyles: ['-' , '-'] 26 | legend: 27 | no balance: 28 | color: 'grey' 29 | linestyle: '-' 30 | balanced: 31 | color: 'orange' 32 | linestyle: '-' 33 | xlim: [0.0, 10.0] 34 | 35 | tilt_control: 36 | title : 'Tilt Control' 37 | desired_y_var: 'DesiredTilt' 38 | actual_y_var: 'Tilt' 39 | runs : ['impulse_force'] 40 | x_label: 'Time (s)' 41 | y_label: 'Tilt (deg)' 42 | colors : ['red', 'blue'] 43 | legend: 44 | desired: 45 | color: 'red' 46 | linestyle: '-' 47 | actual: 48 | color: 'blue' 49 | linestyle: '-' 50 | xlim: [0.0, 10.0] 51 | 52 | velocity_control: 53 | title : 'Velocity Control' 54 | desired_y_var: 'DesiredVelocity' 55 | actual_y_var: 'ForwardVelocity' 56 | runs : ['impulse_force'] 57 | x_label: 'Time (s)' 58 | y_label: 'Velocity (m/s)' 59 | colors : ['red', 'blue'] 60 | legend: 61 | desired: 62 | color: 'red' 63 | linestyle: '-' 64 | actual: 65 | color: 'blue' 66 | linestyle: '-' 67 | xlim: [0.0, 10.0] 68 | 69 | -------------------------------------------------------------------------------- /bobble_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The BalanceSimController yatta yatta TODO docs once I know this works. 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /config/bobble_sim_balance_control.yaml: -------------------------------------------------------------------------------- 1 | bobble: 2 | bobble_balance_controller: 3 | type: bobble_controllers/BalanceSimController 4 | joints: 5 | - right_wheel_hinge 6 | - left_wheel_hinge 7 | InSim: true 8 | ImuName: bno055 9 | ControlLoopFrequency: 500.0 10 | StartingTiltSafetyLimitDegrees: 10.0 11 | MaxTiltSafetyLimitDegrees: 20.0 12 | MotorEffortMax: 0.2 13 | ControllerEffortMax: 0.4 14 | MotorEffortToTorqueSimFactor: 1.0 15 | WheelVelocityAdjustment: 1.0 16 | MadgwickFilterGain: 0.015 17 | MeasuredTiltFilterFrequency: 100.0 18 | MeasuredTiltDotFilterFrequency: 100.0 19 | MeasuredHeadingFilterFrequency: 100.0 20 | MeasuredTurnRateFilterFrequency: 50.0 21 | LeftWheelVelocityFilterFrequency: 10.0 22 | RightWheelVelocityFilterFrequency: 10.0 23 | DesiredForwardVelocityFilterFrequency: 10.0 24 | DesiredTurnRateFilterFrequency: 50.0 25 | MaxVelocityCmd: 4.25 # 3 mph 26 | MaxTurnRateCmd: 6.25 # 150 deg/s 27 | WheelRadiusMeters: 0.05 28 | VelocityCmdScale: 1.15 29 | TurnCmdScale: 6.25 30 | VelocityControlKp: 0.125 31 | VelocityControlKd: 0.05 32 | VelocityControlDerivFilter: 20.0 33 | VelocityControlKi: 0.01 34 | VelocityControlAlphaFilter: 0.99 35 | VelocityControlMaxIntegralOutput: 0.025 36 | VelocityControlOutputLimitDegrees: 8.5 37 | TiltControlKp: 2.5 38 | TiltControlKd: 0.5 39 | TiltControlAlphaFilter: 0.05 40 | TiltOffset: 0.0 41 | TiltDotOffset: 0.0 42 | RollDotOffset: 0.0 43 | YawDotOffset: 0.0 44 | TurningControlKp: 0.25 45 | TurningControlKi: 0.05 46 | TurningControlKd: 0.0 47 | TurningControlDerivFilter: 50.0 48 | TurningOutputFilter: 0.5 49 | 50 | -------------------------------------------------------------------------------- /docs/imgs/BobbleBotGazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/BobbleBotGazebo.png -------------------------------------------------------------------------------- /docs/imgs/BobbleCAD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/BobbleCAD.png -------------------------------------------------------------------------------- /docs/imgs/DifferentialDrive.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/DifferentialDrive.png -------------------------------------------------------------------------------- /docs/imgs/JoystickControls.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/JoystickControls.png -------------------------------------------------------------------------------- /docs/imgs/TiltControl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/TiltControl.png -------------------------------------------------------------------------------- /docs/imgs/VelocityControl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/docs/imgs/VelocityControl.png -------------------------------------------------------------------------------- /include/bobble_controllers/BalanceBaseController.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Document this if it actually works... 3 | * Mike 4 | *******************************************************************************/ 5 | 6 | #ifndef BOBBLE_CONTROLLERS_BALANCE_CONTROLLER_H 7 | #define BOBBLE_CONTROLLERS_BALANCE_CONTROLLER_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace bobble_controllers { 23 | 24 | class BalanceBaseController { 25 | public: 26 | BalanceBaseController(void){}; 27 | ~BalanceBaseController(void); 28 | void init(ros::NodeHandle& nh); 29 | void reset(); 30 | void update(); 31 | void runSubscriber(); 32 | int getControlMode() { return state.ActiveControlMode; }; 33 | 34 | protected: 35 | ros::NodeHandle node_; 36 | bool run_thread_; 37 | realtime_tools::RealtimePublisher* pub_bobble_status_; 38 | ros::Subscriber sub_command_; 39 | std::mutex control_command_mutex_; 40 | std::thread* subscriber_thread_; 41 | bobble_controllers::BalanceControllerConfig config; 42 | bobble_controllers::BalanceControllerCommands received_commands; 43 | bobble_controllers::BalanceControllerCommands processed_commands; 44 | bobble_controllers::BalanceControllerState state; 45 | bobble_controllers::BalanceControllerOutputs outputs; 46 | bobble_controllers::BalanceControllerFilters filters; 47 | bobble_controllers::BalancePIDControllers pid_controllers; 48 | virtual void estimateState() = 0; 49 | virtual void sendMotorCommands() = 0; 50 | virtual void loadConfig(); 51 | void setupFilters(); 52 | void setupControllers(); 53 | void runStateLogic(); 54 | void subscriberCallBack(const bobble_controllers::ControlCommands::ConstPtr &cmd); 55 | void cmdVelCallback(const geometry_msgs::Twist& command); 56 | void idleMode(); 57 | void diagnosticMode(); 58 | void startupMode(); 59 | void balanceMode(); 60 | void unpackParameter(std::string parameterName, double &referenceToParameter, double defaultValue); 61 | void unpackParameter(std::string parameterName, std::string &referenceToParameter, std::string defaultValue); 62 | void unpackFlag(std::string parameterName, bool &referenceToFlag, bool defaultValue); 63 | double limit(double cmd, double max); 64 | 65 | private: 66 | void populateImuData(); 67 | void clearCommandState(bobble_controllers::BalanceControllerCommands& cmds); 68 | void populateCommands(); 69 | void write_controller_status_msg(); 70 | void applyFilters(); 71 | void applySafety(); 72 | }; 73 | } 74 | #endif 75 | -------------------------------------------------------------------------------- /include/bobble_controllers/BalanceControllerData.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mike on 5/11/19. 3 | // 4 | 5 | #ifndef SRC_BALANCE_CONTROLLER_DATA_H 6 | #define SRC_BALANCE_CONTROLLER_DATA_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "bobble_controllers/MadgwickAHRS.h" 12 | #include 13 | #include 14 | 15 | namespace bobble_controllers { 16 | 17 | /// Controller real-time data. 18 | /// HW interface data. 19 | class BalanceControllerRtData { 20 | public: 21 | BalanceControllerRtData() : 22 | AccelX(0.0), AccelY(0.0), AccelZ(0.0), 23 | RollDot(0.0), TiltDot(0.0), TurnDot(0.0), 24 | LeftMotorPosition(0.0), LeftMotorVelocity(0.0), LeftMotorEffort(0.0), 25 | RightMotorPosition(0.0), RightMotorVelocity(0.0), RightMotorEffort(0.0){}; 26 | double AccelX; 27 | double AccelY; 28 | double AccelZ; 29 | double RollDot; 30 | double TiltDot; 31 | double TurnDot; 32 | double LeftMotorPosition; 33 | double LeftMotorVelocity; 34 | double LeftMotorEffort; 35 | double RightMotorPosition; 36 | double RightMotorVelocity; 37 | double RightMotorEffort; 38 | }; 39 | 40 | /// Controller configuration parameters. 41 | /// These are set at init via yml 42 | class BalanceControllerConfig { 43 | public: 44 | double ControlLoopFrequency; 45 | double StartingTiltSafetyLimitDegrees; 46 | double MaxTiltSafetyLimitDegrees; 47 | double ControllerEffortMax; 48 | double MotorEffortToTorqueSimFactor; 49 | double WheelVelocityAdjustment; 50 | double MadgwickFilterGain; 51 | double MeasuredTiltFilterFrequency; 52 | double MeasuredTiltDotFilterFrequency; 53 | double MeasuredHeadingFilterFrequency; 54 | double MeasuredTurnRateFilterFrequency; 55 | double LeftWheelVelocityFilterFrequency; 56 | double RightWheelVelocityFilterFrequency; 57 | double DesiredForwardVelocityFilterFrequency; 58 | double DesiredTurnRateFilterFrequency; 59 | double MaxVelocityCmd; 60 | double MaxTurnRateCmd; 61 | double WheelRadiusMeters; 62 | double VelocityCmdScale; 63 | double TurnCmdScale; 64 | double VelocityControlKp; 65 | double VelocityControlKd; 66 | double VelocityControlDerivFilter; 67 | double VelocityControlKi; 68 | double VelocityControlAlphaFilter; 69 | double VelocityControlMaxIntegralOutput; 70 | double VelocityControlOutputLimitDegrees; 71 | double TiltControlKp; 72 | double TiltControlKd; 73 | double TiltControlAlphaFilter; 74 | double TiltOffset; 75 | double TiltDotOffset; 76 | double RollDotOffset; 77 | double YawDotOffset; 78 | double TurningControlKp; 79 | double TurningControlKi; 80 | double TurningControlKd; 81 | double TurningControlDerivFilter; 82 | double TurningOutputFilter; 83 | }; 84 | 85 | /// Controller commands. 86 | /// These are sent in via external command. 87 | class BalanceControllerCommands { 88 | public: 89 | bool StartupCmd; 90 | bool IdleCmd; 91 | bool DiagnosticCmd; 92 | double DesiredVelocityRaw; 93 | double DesiredTurnRateRaw; 94 | double DesiredVelocity; 95 | double DesiredTurnRate; 96 | }; 97 | 98 | /// Controller outputs. 99 | class BalanceControllerOutputs { 100 | public: 101 | double TiltEffort; 102 | double HeadingEffort; 103 | double LeftMotorEffortCmd; 104 | double RightMotorEffortCmd; 105 | }; 106 | 107 | /// Controller state parameters. 108 | /// These are updated continually at run-time 109 | class BalanceControllerState { 110 | public: 111 | int ActiveControlMode; 112 | double MeasuredTilt; 113 | double MeasuredHeading; 114 | double MeasuredRoll; 115 | double MeasuredRollDot; 116 | double MeasuredTiltDot; 117 | double MeasuredTurnRate; 118 | double FilteredRollDot; 119 | double FilteredTiltDot; 120 | double FilteredTurnRate; 121 | double MeasuredRightMotorPosition; 122 | double MeasuredRightMotorVelocity; 123 | double MeasuredLeftMotorPosition; 124 | double MeasuredLeftMotorVelocity; 125 | double ForwardVelocity; 126 | double DesiredTilt; 127 | double Tilt; 128 | double TiltDot; 129 | double Heading; 130 | double TurnRate; 131 | double LeftWheelVelocity; 132 | double RightWheelVelocity; 133 | BalanceControllerCommands Cmds; 134 | }; 135 | 136 | /// Controller modes 137 | enum ControlModes { 138 | IDLE = 0, 139 | STARTUP = 1, 140 | BALANCE = 2, 141 | DIAGNOSTIC = 3 142 | }; 143 | 144 | /// Controller filters 145 | class BalanceControllerFilters { 146 | public: 147 | LowPassFilter MeasuredTiltFilter; 148 | LowPassFilter MeasuredTiltDotFilter; 149 | LowPassFilter MeasuredHeadingFilter; 150 | LowPassFilter MeasuredTurnRateFilter; 151 | LowPassFilter LeftWheelVelocityFilter; 152 | LowPassFilter RightWheelVelocityFilter; 153 | LowPassFilter DesiredForwardVelocityFilter; 154 | LowPassFilter DesiredTurnRateFilter; 155 | }; 156 | 157 | 158 | /// PID Controllers 159 | class BalancePIDControllers { 160 | public: 161 | BalancePIDControllers() : 162 | VelocityControlPID(0.0, 0.0, 0.0, 0.0, 0.0), 163 | TiltControlPID(0.0, 0.0, 0.0, 0.0, 0.0), 164 | TurningControlPID(0.0, 0.0, 0.0, 0.0, 0.0) 165 | {} 166 | PidControl VelocityControlPID; 167 | PidControl TiltControlPID; 168 | PidControl TurningControlPID; 169 | }; 170 | } 171 | 172 | #endif //SRC_BALANCE_CONTROLLER_DATA_H 173 | -------------------------------------------------------------------------------- /include/bobble_controllers/BalanceSimController.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Gazebo ROS interface for Bobble-Bot's balance controller. 3 | * 4 | *******************************************************************************/ 5 | 6 | #ifndef BOBBLE_CONTROLLERS_BALANCE_SIM_CONTROLLER_H 7 | #define BOBBLE_CONTROLLERS_BALANCE_SIM_CONTROLLER_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace bobble_controllers { 26 | 27 | class BalanceSimController : public bobble_controllers::BalanceBaseController, 28 | public controller_interface::Controller 29 | { 30 | public: 31 | BalanceSimController(){}; 32 | ~BalanceSimController(){}; 33 | 34 | void starting(const ros::Time &time); 35 | void update(const ros::Time &time, const ros::Duration &duration); 36 | 37 | protected: 38 | virtual bool initRequest(hardware_interface::RobotHW* robot_hw, 39 | ros::NodeHandle& root_nh, 40 | ros::NodeHandle& controller_nh, 41 | ClaimedResources& claimed_resources) override; 42 | 43 | 44 | virtual void loadConfig(); 45 | virtual void estimateState(); 46 | virtual void sendMotorCommands(); 47 | 48 | private: 49 | hardware_interface::RobotHW *robot_; 50 | std::vector joints_; 51 | ros::Subscriber sub_imu_sensor_; 52 | void imuCB(const sensor_msgs::Imu::ConstPtr &imuData); 53 | }; 54 | } 55 | #endif 56 | -------------------------------------------------------------------------------- /include/bobble_controllers/Filter.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////// 2 | /// @defgroup DSP Digital Signal Processing Module 3 | /// The DSP module will contain classes for doing 4 | /// digital signal processing. The first class to 5 | /// be implemented will be the filter base class. 6 | /// We will then derive two filters from the base 7 | /// class to cover discrete FIR and IIR digital 8 | /// filters. 9 | /// 10 | /// @author 11 | /// $Author: Mike Moore $ 12 | /// 13 | /// Contact: mike.mike@so.engineering 14 | /// 15 | /// Created on: Sat May 31 2014 16 | /// 17 | /////////////////////////////////////////////////////////////// 18 | #ifndef FILTER_HH 19 | #define FILTER_HH 20 | 21 | /// @note This is the maximum allowable size for any filter 22 | /// built with this class. It is expected that twenty 23 | /// will be more than enough for our applications. 24 | #define MAX_FILTER_SIZE 20 25 | 26 | /////////////////////////////////////////////////////////////// 27 | /// @class Filter 28 | /// @ingroup DSP 29 | /// @brief Base class for a generic digital filter 30 | /// implementation. The implementation relies on the 31 | /// following discrete equation for the digital output 32 | /// signal y[n] as a function of x[n]: \par 33 | /// 34 | ///
35 | /// \f$ a[0]*y[n] = b[0]x[n]+b[1]x[n-1]+...+b[N]x[n-B+1] 36 | /// -a[1]y[n-1]-...-a[N]y[n-N+1] \f$ 37 | ///
38 | /// 39 | /// The class allows you to set the a and b weights in 40 | /// order to construct any digital filter. Here is an example 41 | /// of its use to replicate a three point moving average 42 | /// filter. 43 | /// 44 | /// @image html avg_filter_verif.png "Moving Average Filter" 45 | /////////////////////////////////////////////////////////////// 46 | class Filter { 47 | 48 | public: 49 | ////////////////////////////////////////////////////////// 50 | /// @brief This constructor will construct the filter 51 | /// with all the necessary parameters to build a 52 | /// custom filter. 53 | //////////////////////////////////////////////////////////// 54 | Filter(double numInWeights, double* inWeights, 55 | double numOutWeights, double* outWeights); 56 | ////////////////////////////////////////////////////////// 57 | /// @brief The default c'tor constructs the Filter base 58 | /// class. 59 | //////////////////////////////////////////////////////////// 60 | Filter(); 61 | ////////////////////////////////////////////////////////// 62 | /// @brief The default d'tor destructs the Filter base 63 | /// class. 64 | //////////////////////////////////////////////////////////// 65 | ~Filter(); 66 | //////////////////////////////////////////////////////////// 67 | /// @brief Main filter routine. This is a virtual function 68 | /// and it is expected that filters which derive 69 | /// from this base class will implement their own 70 | /// version of this routine. 71 | /// @param inputValue input value. 72 | /// @return Output from the filter 73 | //////////////////////////////////////////////////////////// 74 | virtual double filter(double inputValue); 75 | //////////////////////////////////////////////////////////// 76 | /// @brief An accessor function to get the current input 77 | /// buffer of the filter. 78 | /// @return The current input buffer of the filter. 79 | //////////////////////////////////////////////////////////// 80 | inline double* GetCurrentInputBuffer(void){ 81 | return _inputBuffer; } 82 | //////////////////////////////////////////////////////////// 83 | /// @brief An accessor function to get the current output 84 | /// buffer of the filter. 85 | /// @return The current output buffer of the filter. 86 | //////////////////////////////////////////////////////////// 87 | inline double* GetCurrentOutputBuffer(void){ 88 | return _outputBuffer; } 89 | //////////////////////////////////////////////////////////// 90 | /// @brief An accessor function to get the filter's input 91 | /// weights. 92 | /// @return The filters input weights 93 | //////////////////////////////////////////////////////////// 94 | inline double* GetInputWeights(void){ 95 | return _inputWeights; } 96 | //////////////////////////////////////////////////////////// 97 | /// @brief An accessor function to get the filter's output 98 | /// weights. 99 | /// @return The filters output weights 100 | inline double* GetOutputWeights(void){ 101 | return _outputWeights; } 102 | 103 | protected: 104 | //////////////////////////////////////////////////////////// 105 | /// @brief Initializes the input and output buffers of the 106 | /// filter to 0.0; 107 | /// @param buff -- The input or output buffer it initialize. 108 | /// @param buff_size -- Size of the buffer 109 | //////////////////////////////////////////////////////////// 110 | void initBuffer(double* buff, unsigned int buff_size); 111 | //////////////////////////////////////////////////////////// 112 | /// @brief Buffer holding the input signal values. 113 | //////////////////////////////////////////////////////////// 114 | double _inputBuffer[MAX_FILTER_SIZE]; 115 | //////////////////////////////////////////////////////////// 116 | /// @brief Buffer holding the output signal values. 117 | //////////////////////////////////////////////////////////// 118 | double _outputBuffer[MAX_FILTER_SIZE]; 119 | //////////////////////////////////////////////////////////// 120 | /// @brief The number of input weights. 121 | //////////////////////////////////////////////////////////// 122 | unsigned int _numInWeights; 123 | //////////////////////////////////////////////////////////// 124 | /// @brief Weights to be applied to the input signal and 125 | /// delayed values of the input signal in order to 126 | /// generate the output signal. 127 | //////////////////////////////////////////////////////////// 128 | double _inputWeights[MAX_FILTER_SIZE]; 129 | //////////////////////////////////////////////////////////// 130 | /// @brief The number of output weights. 131 | //////////////////////////////////////////////////////////// 132 | unsigned int _numOutWeights; 133 | //////////////////////////////////////////////////////////// 134 | /// @brief Weights to be applied to the filter output signal 135 | /// and delayed values of the output signal in order 136 | /// to generate the next filter output. 137 | //////////////////////////////////////////////////////////// 138 | double _outputWeights[MAX_FILTER_SIZE]; 139 | 140 | 141 | }; 142 | 143 | class LPFilter : public Filter { 144 | public: 145 | LPFilter(); 146 | ~LPFilter(){}; 147 | void setGain(double gain); 148 | private: 149 | double _Gain; 150 | }; 151 | 152 | 153 | /* 154 | * @brief The c'tor constructs the filter weights 155 | * necessary for a low pass filter. 156 | */ 157 | inline LPFilter::LPFilter() : _Gain(0.0) 158 | { 159 | _numInWeights = 1; 160 | _numOutWeights = 2; 161 | _inputWeights[0] = 1.0-_Gain; 162 | _outputWeights[0] = 1; 163 | _outputWeights[1] = _Gain; 164 | } 165 | 166 | inline void LPFilter::setGain(double gain) 167 | { 168 | _Gain = gain; 169 | _inputWeights[0] = 1.0-_Gain; 170 | _outputWeights[1] = _Gain; 171 | } 172 | 173 | 174 | #endif // FILTER_HH 175 | -------------------------------------------------------------------------------- /include/bobble_controllers/HighPassFilter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by james on 3/27/20. 3 | // 4 | 5 | #ifndef SRC_HIGHPASSFILTER_H 6 | #define SRC_HIGHPASSFILTER_H 7 | 8 | #include "Filter.h" 9 | #include 10 | 11 | class HighPassFilter : public Filter 12 | { 13 | public: 14 | ////////////////////////////////////////////////////////// 15 | /// @brief The high pass filter takes three arguments 16 | /// and then calculates the correct coefficients 17 | /// 18 | /// @param Ts: The sample period in seconds 19 | /// @param fc: The cutoff frequency in Hz 20 | /// @param zeta: The damping ratio (unitless) - default is 21 | /// 1.0 for no overshoot 22 | //////////////////////////////////////////////////////////// 23 | HighPassFilter(double Ts, double fc, double zeta); 24 | 25 | ////////////////////////////////////////////////////////// 26 | /// @brief The high pass filter takes three arguments 27 | /// and then calculates the correct coefficients 28 | /// This default constructor has hardcoded params 29 | /// 30 | //////////////////////////////////////////////////////////// 31 | HighPassFilter() : HighPassFilter(0.002, 50, 1.0){}; 32 | 33 | void resetFilterParameters(double Ts, double fc, double zeta); 34 | }; 35 | 36 | 37 | #endif //SRC_HIGHPASSFILTER_H 38 | -------------------------------------------------------------------------------- /include/bobble_controllers/LowPassFilter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by james on 3/28/20. 3 | // 4 | 5 | #ifndef SRC_LOWPASSFILTER_H 6 | #define SRC_LOWPASSFILTER_H 7 | 8 | #include 9 | #include 10 | 11 | class LowPassFilter : public Filter 12 | { 13 | public: 14 | ////////////////////////////////////////////////////////// 15 | /// @brief The low pass filter takes three arguments 16 | /// and then calculates the correct coefficients 17 | /// 18 | /// @param Ts: The sample period in seconds 19 | /// @param fc: The cutoff frequency in Hz 20 | /// @param zeta: The damping ratio (unitless) - default is 21 | /// 1.0 for no overshoot 22 | //////////////////////////////////////////////////////////// 23 | LowPassFilter(double Ts, double fc, double zeta); 24 | 25 | ////////////////////////////////////////////////////////// 26 | /// @brief The low pass filter takes three arguments 27 | /// and then calculates the correct coefficients 28 | /// This default constructor has hardcoded params 29 | /// 30 | //////////////////////////////////////////////////////////// 31 | LowPassFilter() : LowPassFilter(0.002, 50.0, 1.0){}; 32 | 33 | void resetFilterParameters(double Ts, double fc, double zeta); 34 | }; 35 | #endif //SRC_LOWPASSFILTER_H 36 | -------------------------------------------------------------------------------- /include/bobble_controllers/MadgwickAHRS.h: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MadgwickAHRS.h 3 | //===================================================================================================== 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | #ifndef MadgwickAHRS_h 14 | #define MadgwickAHRS_h 15 | 16 | //---------------------------------------------------------------------------------------------------- 17 | // Variable declaration 18 | 19 | extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 20 | 21 | //--------------------------------------------------------------------------------------------------- 22 | // Function declarations 23 | 24 | 25 | void MadgwickAHRSupdate(float beta, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 26 | void MadgwickAHRSupdateIMU(float beta, float gx, float gy, float gz, float ax, float ay, float az); 27 | 28 | #endif 29 | //===================================================================================================== 30 | // End of file 31 | //===================================================================================================== 32 | -------------------------------------------------------------------------------- /include/bobble_controllers/PIDFilters.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by james on 3/29/20. 3 | // 4 | 5 | #ifndef SRC_PIDFILTERS_H 6 | #define SRC_PIDFILTERS_H 7 | 8 | #include "Filter.h" 9 | #include 10 | 11 | class DerivativeFilter : public Filter 12 | { 13 | public: 14 | ////////////////////////////////////////////////////////// 15 | /// @brief The derivative filter takes three arguments 16 | /// and then calculates the correct coefficients 17 | /// 18 | /// @param Ts: The sample period in seconds 19 | /// @param fc: The cutoff frequency in Hz 20 | /// @param Kd: The damping gain (unitless) - default is 1.0 21 | /// 22 | //////////////////////////////////////////////////////////// 23 | DerivativeFilter(double Ts, double fc, double Kd); 24 | 25 | ////////////////////////////////////////////////////////// 26 | /// @brief The derivative filter takes three arguments 27 | /// and then calculates the correct coefficients 28 | /// This default constructor has hardcoded params 29 | /// 30 | //////////////////////////////////////////////////////////// 31 | DerivativeFilter() : DerivativeFilter(0.002, 50, 1.0){}; 32 | 33 | void resetFilterParameters(double Ts, double fc, double Kd); 34 | }; 35 | 36 | class IntegralFilter : public Filter 37 | { 38 | public: 39 | ////////////////////////////////////////////////////////// 40 | /// @brief The integral filter takes two arguments 41 | /// and then calculates the correct coefficients 42 | /// 43 | /// @param Ts: The sample period in seconds 44 | /// @param Ki: The integral gain (unitless) - default is 1.0 45 | /// 46 | //////////////////////////////////////////////////////////// 47 | IntegralFilter(double Ts, double Ki); 48 | 49 | ////////////////////////////////////////////////////////// 50 | /// @brief The high pass filter takes two arguments 51 | /// and then calculates the correct coefficients 52 | /// This default constructor has hardcoded params 53 | /// 54 | //////////////////////////////////////////////////////////// 55 | IntegralFilter() : IntegralFilter(0.002, 1.0){}; 56 | 57 | void resetFilterParameters(double Ts, double Ki); 58 | }; 59 | 60 | #endif //SRC_PIDFILTERS_H 61 | -------------------------------------------------------------------------------- /include/bobble_controllers/PidControl.h: -------------------------------------------------------------------------------- 1 | #ifndef PIDCONTROL_H 2 | #define PIDCONTROL_H 3 | 4 | #include 5 | 6 | class PidControl{ 7 | public: 8 | PidControl(double p, double i, double d, double fc, double sampleTime); 9 | PidControl(double p, double i, double d, double fc, double sampleTime, double f); 10 | void setP(double p); 11 | void setI(double i); 12 | void setD(double d, double fc); 13 | void setF(double f); 14 | void setPID(double p, double i, double d, double fc, double sampleTime); 15 | void setPID(double p, double i, double d, double fc, double sampleTime, double f); 16 | void setMaxIOutput(double); 17 | void setOutputLimits(double); 18 | void setOutputLimits(double,double); 19 | void setDirection(bool); 20 | void setSetpoint(double); 21 | void reset(); 22 | void setOutputRampRate(double); 23 | void setExternalDerivativeError(double*); 24 | void setSetpointRange(double); 25 | void setOutputFilter(double); 26 | double getOutput(); 27 | double getOutput(double); 28 | double getOutput(double, double); 29 | 30 | private: 31 | double clamp(double, double, double); 32 | bool bounded(double, double, double); 33 | void checkSigns(); 34 | void init(); 35 | double P; 36 | double I; 37 | double D; 38 | double Fc; 39 | double F; 40 | double Ts; 41 | 42 | DerivativeFilter derivativeFilter; 43 | IntegralFilter integralFilter; 44 | 45 | double maxIOutput; 46 | double maxError; 47 | double errorSum; 48 | double* extDerivError; 49 | 50 | double maxOutput; 51 | double minOutput; 52 | 53 | double setpoint; 54 | 55 | double lastActual; 56 | 57 | bool firstRun; 58 | bool reversed; 59 | bool useExternalDerivError; 60 | 61 | double outputRampRate; 62 | double lastOutput; 63 | 64 | double outputFilter; 65 | 66 | double setpointRange; 67 | }; 68 | #endif 69 | -------------------------------------------------------------------------------- /launch/apply_impulse_force.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/bobble_balance_state_hold.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/drive_square.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/drive_square_js_cmds.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/launch/drive_square_js_cmds.bag -------------------------------------------------------------------------------- /launch/run_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/run_sim_with_joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /msg/BobbleBotStatus.msg: -------------------------------------------------------------------------------- 1 | int32 ControlMode 2 | float32 MeasuredTiltDot 3 | float32 MeasuredTurnRate 4 | float32 FilteredTiltDot 5 | float32 FilteredTurnRate 6 | float32 Tilt 7 | float32 TiltRate 8 | float32 Heading 9 | float32 TurnRate 10 | float32 ForwardVelocity 11 | float32 DesiredVelocity 12 | float32 DesiredTilt 13 | float32 DesiredTurnRate 14 | float32 LeftMotorPosition 15 | float32 LeftMotorVelocity 16 | float32 RightMotorPosition 17 | float32 RightMotorVelocity 18 | float32 TiltEffort 19 | float32 HeadingEffort 20 | float32 LeftMotorEffortCmd 21 | float32 RightMotorEffortCmd 22 | -------------------------------------------------------------------------------- /msg/ControlCommands.msg: -------------------------------------------------------------------------------- 1 | bool StartupCmd 2 | bool IdleCmd 3 | bool DiagnosticCmd 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bobble_controllers 4 | 1.0.0 5 | Holds the real-time balance controller for BobbleBot 6 | Mike Moore 7 | 8 | tbd 9 | tbd 10 | Mike Moore 11 | 12 | catkin 13 | 14 | control_msgs 15 | controller_interface 16 | controller_manager 17 | message_generation 18 | pluginlib 19 | rospy 20 | rostest 21 | sensor_msgs 22 | std_msgs 23 | 24 | gazebo_ros 25 | gazebo_ros_control 26 | gazebo_ros_pkgs 27 | control_msgs 28 | controller_interface 29 | controller_manager 30 | joint_state_controller 31 | message_runtime 32 | pluginlib 33 | rospy 34 | rostest 35 | sensor_msgs 36 | std_msgs 37 | python-seaborn 38 | 39 | xacro 40 | rosunit 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['analysis_tools'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) -------------------------------------------------------------------------------- /src/BalanceSimController.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace bobble_controllers { 6 | 7 | void BalanceSimController::loadConfig() { 8 | unpackParameter("MotorEffortToTorqueSimFactor", config.MotorEffortToTorqueSimFactor, 0.832); 9 | BalanceBaseController::loadConfig(); 10 | } 11 | 12 | bool BalanceSimController::initRequest(hardware_interface::RobotHW* robot_hw, 13 | ros::NodeHandle& root_nh, 14 | ros::NodeHandle& controller_nh, 15 | ClaimedResources& claimed_resources) 16 | { 17 | /// check if construction finished cleanly 18 | if (state_ != CONSTRUCTED){ 19 | ROS_ERROR("Cannot initialize this controller because it failed to be constructed"); 20 | return false; 21 | } 22 | node_ = controller_nh; 23 | loadConfig(); 24 | BalanceBaseController::setupFilters(); 25 | BalanceBaseController::setupControllers(); 26 | pub_bobble_status_ = new realtime_tools::RealtimePublisher(root_nh, 27 | "bobble_balance_controller/bb_controller_status", 1); 28 | run_thread_ = true; 29 | subscriber_thread_ = new std::thread(&BalanceBaseController::runSubscriber, this); 30 | /// get a pointer to the effort interface 31 | hardware_interface::EffortJointInterface* effort_hw = robot_hw->get(); 32 | if (!effort_hw) 33 | { 34 | ROS_ERROR("This controller requires a hardware interface of type hardware_interface::EffortJointInterface."); 35 | return false; 36 | } 37 | /// Load the joint names from the controller config file 38 | XmlRpc::XmlRpcValue joint_names; 39 | if (!node_.getParam("joints", joint_names)) { 40 | ROS_ERROR("No 'joints' in controller. (namespace: %s)", 41 | node_.getNamespace().c_str()); 42 | return false; 43 | } 44 | if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) { 45 | ROS_ERROR("'joints' is not a struct. (namespace: %s)", 46 | node_.getNamespace().c_str()); 47 | return false; 48 | } 49 | for (int i = 0; i < joint_names.size(); i++) { 50 | XmlRpc::XmlRpcValue &name_value = joint_names[i]; 51 | if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString) { 52 | ROS_ERROR("joints are not strings. (namespace: %s)", 53 | node_.getNamespace().c_str()); 54 | return false; 55 | } 56 | hardware_interface::JointHandle j = robot_hw->get()->getHandle((std::string) name_value); 57 | joints_.push_back(j); 58 | } 59 | sub_imu_sensor_ = node_.subscribe("/imu_bosch/data_raw", 1, &BalanceSimController::imuCB, this); 60 | state_ = INITIALIZED; 61 | return true; 62 | } 63 | 64 | void BalanceSimController::starting(const ros::Time &time) { 65 | BalanceBaseController::reset(); 66 | // Reset Madgwick Q on start is a sim only thing. The sim 67 | // resets the orientation on transition from Idle to Balance 68 | q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; 69 | } 70 | 71 | void BalanceSimController::update(const ros::Time &time, const ros::Duration &duration) { 72 | /// Reset the quaternion every time we go to idle in sim 73 | if (state.ActiveControlMode == bobble_controllers::ControlModes::IDLE) { 74 | q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; 75 | } 76 | BalanceBaseController::update(); 77 | } 78 | 79 | void BalanceSimController::imuCB(const sensor_msgs::Imu::ConstPtr &imuData) { 80 | state.MeasuredTiltDot = imuData->angular_velocity.y; 81 | state.MeasuredTurnRate = imuData->angular_velocity.z; 82 | MadgwickAHRSupdateIMU(config.MadgwickFilterGain, imuData->angular_velocity.x, 83 | imuData->angular_velocity.y, imuData->angular_velocity.z, 84 | imuData->linear_acceleration.x, imuData->linear_acceleration.y, 85 | imuData->linear_acceleration.z); 86 | tf::Quaternion q(q0, q1, q2, q3); 87 | tf::Matrix3x3 m(q); 88 | m.getRPY(state.MeasuredHeading, state.MeasuredTilt, state.MeasuredRoll); 89 | state.MeasuredTilt *= -1.0; 90 | } 91 | 92 | void BalanceSimController::estimateState() 93 | { 94 | state.MeasuredLeftMotorPosition = joints_[1].getPosition(); 95 | state.MeasuredRightMotorPosition = joints_[0].getPosition(); 96 | state.MeasuredLeftMotorVelocity = joints_[1].getVelocity(); 97 | state.MeasuredRightMotorVelocity = joints_[0].getVelocity(); 98 | } 99 | 100 | void BalanceSimController::sendMotorCommands() 101 | { 102 | joints_[0].setCommand(outputs.RightMotorEffortCmd * config.MotorEffortToTorqueSimFactor); 103 | joints_[1].setCommand(outputs.LeftMotorEffortCmd * config.MotorEffortToTorqueSimFactor); 104 | } 105 | 106 | } 107 | 108 | PLUGINLIB_EXPORT_CLASS(bobble_controllers::BalanceSimController, controller_interface::ControllerBase 109 | ) 110 | -------------------------------------------------------------------------------- /src/Filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | ////////////////////////////////////////////////////////// 4 | /// @brief The c'tor constructs the class members. 5 | //////////////////////////////////////////////////////////// 6 | Filter::Filter(double numInWeights, double* inWeights, 7 | double numOutWeights, double* outWeights) : 8 | _numInWeights(numInWeights), 9 | _numOutWeights(numOutWeights) 10 | { 11 | /// First initialize all weights to 0.0 12 | for (unsigned int i = 0; i0; i--){ 75 | _inputBuffer[i] = _inputBuffer[i-1]; 76 | } 77 | _inputBuffer[0] = inputValue; 78 | /// Buffer the output values and previous output values. 79 | for(unsigned int i=_numOutWeights; i>0; i--){ 80 | _outputBuffer[i] = _outputBuffer[i-1]; 81 | } 82 | for(unsigned int i=0; i<_numInWeights; i++){ 83 | inputContribution += _inputWeights[i]*_inputBuffer[i]; 84 | } 85 | for(unsigned int i=1; i<_numOutWeights; i++){ 86 | outputContribution += _outputWeights[i]*_outputBuffer[i]; 87 | } 88 | /// @note calculate the current filter output based on previous 89 | /// inputs and previous outputs. 90 | _outputBuffer[0] = (1.0/_outputWeights[0])*(inputContribution - outputContribution); 91 | return _outputBuffer[0]; 92 | } 93 | -------------------------------------------------------------------------------- /src/HighPassFilter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by james on 3/27/20. 3 | // 4 | 5 | #include 6 | 7 | HighPassFilter::HighPassFilter(double Ts, double fc, double zeta) { 8 | resetFilterParameters(Ts, fc, zeta); 9 | } 10 | 11 | void HighPassFilter::resetFilterParameters(double Ts, double fc, double zeta) 12 | { 13 | /// Initialize buffers to 0 14 | initBuffer(_inputBuffer, 3); 15 | initBuffer(_outputBuffer, 3); 16 | 17 | _numInWeights = 3; 18 | _numOutWeights = 3; 19 | double wc = (2.0 * M_PI * fc); 20 | /// The following calculations are the implementation of a second order high pass filter 21 | /// converted from continuous space to discrete space using the bilinear transform. 22 | /// The continuous time filter is s^2 / (s^2 + 2*zeta*wc*s + wc^2) 23 | double numeratorWeights[3] = {4.0, -8.0, 4.0}; 24 | double denominatorWeights[3] = {Ts*Ts*wc*wc+4*Ts*wc*zeta+4, 25 | 2*Ts*Ts*wc*wc-8, 26 | Ts*Ts*wc*wc-4*Ts*wc*zeta+4}; 27 | 28 | /// First initialize all weights to 0.0 29 | for (unsigned int i = 0; i 6 | 7 | LowPassFilter::LowPassFilter(double Ts, double fc, double zeta) { 8 | resetFilterParameters(Ts, fc, zeta); 9 | } 10 | 11 | void LowPassFilter::resetFilterParameters(double Ts, double fc, double zeta) { 12 | /// Initialize buffers to 0 13 | initBuffer(_inputBuffer, 3); 14 | initBuffer(_outputBuffer, 3); 15 | 16 | _numInWeights = 3; 17 | _numOutWeights = 3; 18 | double wc = 2.0 * M_PI * fc; 19 | /// The following calculations are the implementation of a second order high pass filter 20 | /// converted from continuous space to discrete space using the bilinear transform. 21 | /// The continuous time filter is 1 / (s^2 + 2*zeta*wc*s + wc^2) 22 | double numeratorWeights[3] = {Ts*Ts*wc*wc, 2*Ts*Ts*wc*wc, Ts*Ts*wc*wc}; 23 | double denominatorWeights[3] = {Ts*Ts*wc*wc+4*Ts*wc*zeta+4, 24 | 2*Ts*Ts*wc*wc-8, 25 | Ts*Ts*wc*wc-4*Ts*wc*zeta+4}; 26 | 27 | /// First initialize all weights to 0.0 28 | for (unsigned int i = 0; i 20 | 21 | //--------------------------------------------------------------------------------------------------- 22 | // Definitions 23 | 24 | #define sampleFreq 500.0f // sample frequency in Hz 25 | 26 | //--------------------------------------------------------------------------------------------------- 27 | // Variable definitions 28 | 29 | volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame 30 | 31 | //--------------------------------------------------------------------------------------------------- 32 | // Function declarations 33 | 34 | float invSqrt(float x); 35 | 36 | //==================================================================================================== 37 | // Functions 38 | 39 | //--------------------------------------------------------------------------------------------------- 40 | // AHRS algorithm update 41 | 42 | void MadgwickAHRSupdate(float beta, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { 43 | float recipNorm; 44 | float s0, s1, s2, s3; 45 | float qDot1, qDot2, qDot3, qDot4; 46 | float hx, hy; 47 | float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; 48 | 49 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) 50 | if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { 51 | MadgwickAHRSupdateIMU(beta, gx, gy, gz, ax, ay, az); 52 | return; 53 | } 54 | 55 | // Rate of change of quaternion from gyroscope 56 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 57 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 58 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 59 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 60 | 61 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 62 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 63 | 64 | // Normalise accelerometer measurement 65 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 66 | ax *= recipNorm; 67 | ay *= recipNorm; 68 | az *= recipNorm; 69 | 70 | // Normalise magnetometer measurement 71 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); 72 | mx *= recipNorm; 73 | my *= recipNorm; 74 | mz *= recipNorm; 75 | 76 | // Auxiliary variables to avoid repeated arithmetic 77 | _2q0mx = 2.0f * q0 * mx; 78 | _2q0my = 2.0f * q0 * my; 79 | _2q0mz = 2.0f * q0 * mz; 80 | _2q1mx = 2.0f * q1 * mx; 81 | _2q0 = 2.0f * q0; 82 | _2q1 = 2.0f * q1; 83 | _2q2 = 2.0f * q2; 84 | _2q3 = 2.0f * q3; 85 | _2q0q2 = 2.0f * q0 * q2; 86 | _2q2q3 = 2.0f * q2 * q3; 87 | q0q0 = q0 * q0; 88 | q0q1 = q0 * q1; 89 | q0q2 = q0 * q2; 90 | q0q3 = q0 * q3; 91 | q1q1 = q1 * q1; 92 | q1q2 = q1 * q2; 93 | q1q3 = q1 * q3; 94 | q2q2 = q2 * q2; 95 | q2q3 = q2 * q3; 96 | q3q3 = q3 * q3; 97 | 98 | // Reference direction of Earth's magnetic field 99 | hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; 100 | hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; 101 | _2bx = sqrt(hx * hx + hy * hy); 102 | _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; 103 | _4bx = 2.0f * _2bx; 104 | _4bz = 2.0f * _2bz; 105 | 106 | // Gradient decent algorithm corrective step 107 | s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 108 | s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 109 | s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 110 | s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 111 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 112 | s0 *= recipNorm; 113 | s1 *= recipNorm; 114 | s2 *= recipNorm; 115 | s3 *= recipNorm; 116 | 117 | // Apply feedback step 118 | qDot1 -= beta * s0; 119 | qDot2 -= beta * s1; 120 | qDot3 -= beta * s2; 121 | qDot4 -= beta * s3; 122 | } 123 | 124 | // Integrate rate of change of quaternion to yield quaternion 125 | q0 += qDot1 * (1.0f / sampleFreq); 126 | q1 += qDot2 * (1.0f / sampleFreq); 127 | q2 += qDot3 * (1.0f / sampleFreq); 128 | q3 += qDot4 * (1.0f / sampleFreq); 129 | 130 | // Normalise quaternion 131 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 132 | q0 *= recipNorm; 133 | q1 *= recipNorm; 134 | q2 *= recipNorm; 135 | q3 *= recipNorm; 136 | } 137 | 138 | //--------------------------------------------------------------------------------------------------- 139 | // IMU algorithm update 140 | 141 | void MadgwickAHRSupdateIMU(float beta, float gx, float gy, float gz, float ax, float ay, float az) { 142 | float recipNorm; 143 | float s0, s1, s2, s3; 144 | float qDot1, qDot2, qDot3, qDot4; 145 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; 146 | 147 | // Rate of change of quaternion from gyroscope 148 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 149 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 150 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 151 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 152 | 153 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 154 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 155 | 156 | // Normalise accelerometer measurement 157 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 158 | ax *= recipNorm; 159 | ay *= recipNorm; 160 | az *= recipNorm; 161 | 162 | // Auxiliary variables to avoid repeated arithmetic 163 | _2q0 = 2.0f * q0; 164 | _2q1 = 2.0f * q1; 165 | _2q2 = 2.0f * q2; 166 | _2q3 = 2.0f * q3; 167 | _4q0 = 4.0f * q0; 168 | _4q1 = 4.0f * q1; 169 | _4q2 = 4.0f * q2; 170 | _8q1 = 8.0f * q1; 171 | _8q2 = 8.0f * q2; 172 | q0q0 = q0 * q0; 173 | q1q1 = q1 * q1; 174 | q2q2 = q2 * q2; 175 | q3q3 = q3 * q3; 176 | 177 | // Gradient decent algorithm corrective step 178 | s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; 179 | s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; 180 | s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; 181 | s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; 182 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 183 | s0 *= recipNorm; 184 | s1 *= recipNorm; 185 | s2 *= recipNorm; 186 | s3 *= recipNorm; 187 | 188 | // Apply feedback step 189 | qDot1 -= beta * s0; 190 | qDot2 -= beta * s1; 191 | qDot3 -= beta * s2; 192 | qDot4 -= beta * s3; 193 | } 194 | 195 | // Integrate rate of change of quaternion to yield quaternion 196 | q0 += qDot1 * (1.0f / sampleFreq); 197 | q1 += qDot2 * (1.0f / sampleFreq); 198 | q2 += qDot3 * (1.0f / sampleFreq); 199 | q3 += qDot4 * (1.0f / sampleFreq); 200 | 201 | // Normalise quaternion 202 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 203 | q0 *= recipNorm; 204 | q1 *= recipNorm; 205 | q2 *= recipNorm; 206 | q3 *= recipNorm; 207 | } 208 | 209 | //--------------------------------------------------------------------------------------------------- 210 | // Fast inverse square-root 211 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 212 | 213 | float invSqrt(float x) { 214 | float halfx = 0.5f * x; 215 | float y = x; 216 | long i = *(long*)&y; 217 | i = 0x5f3759df - (i>>1); 218 | y = *(float*)&i; 219 | y = y * (1.5f - (halfx * y * y)); 220 | return y; 221 | } 222 | 223 | //==================================================================================================== 224 | // END OF CODE 225 | //==================================================================================================== 226 | -------------------------------------------------------------------------------- /src/PIDFilters.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by james on 3/29/20. 3 | // 4 | 5 | #include 6 | 7 | DerivativeFilter::DerivativeFilter(double Ts, double fc, double Kd){ 8 | resetFilterParameters(Ts, fc, Kd); 9 | } 10 | 11 | void DerivativeFilter::resetFilterParameters(double Ts, double fc, double Kd) 12 | { 13 | /// Initialize buffers to 0 14 | initBuffer(_inputBuffer, 2); 15 | initBuffer(_outputBuffer, 2); 16 | 17 | _numInWeights = 2; 18 | _numOutWeights = 2; 19 | double wc = (2.0 * M_PI * fc); 20 | /// The following calculations are the implementation of a derivative and first order filter 21 | /// converted from continuous space to discrete space using the bilinear transform. 22 | /// The continuous time filter is s Kd wc / (s + wc) 23 | double numeratorWeights[2] = {2.0*Kd*wc, -2.0*Kd*wc}; 24 | double denominatorWeights[2] = {2.0 + wc*Ts, wc*Ts - 2.0}; 25 | 26 | /// First initialize all weights to 0.0 27 | for (unsigned int i = 0; i 0) 100 | freqs = sample_freq[pos_mask] 101 | power = power[pos_mask] 102 | # Plot the FFT power 103 | ax1.plot(freqs, power) 104 | ax1.grid(True) 105 | ax1.set_xlabel('Frequency [Hz]') 106 | ax1.set_ylabel('Power') 107 | ax1.set_title('Frequency Spectrum') 108 | 109 | fig1 = plt.figure(figsize=(width, height), dpi=dpi) 110 | ax1 = fig1.add_subplot(111) 111 | filtered_sig = butter_lowpass_filter(raw_signal, fs, cutoff_freq, order=order) 112 | ax1.plot(raw_signal.index, raw_signal, label='Raw signal') 113 | ax1.plot(raw_signal.index, filtered_sig, label='Filtered signal') 114 | ax1.set_xlabel('Time (s)') 115 | ax1.set_ylabel(pc['y_label']) 116 | ax1.grid(True) 117 | ax1.legend(loc='best') 118 | ax1.set_title('Raw vs Filtered') 119 | 120 | fig2 = plt.figure(figsize=(width, height), dpi=dpi) 121 | ax1 = fig2.add_subplot(211) 122 | ax2 = fig2.add_subplot(212) 123 | b, a = butter_lowpass(cutoff_freq, fs, order=order) 124 | w, h = freqz(b, a, worN=2000) 125 | ax1.plot((fs * 0.5 / np.pi) * w, abs(h)) 126 | ax1.plot(cutoff_freq, 0.5*np.sqrt(2), 'ko') 127 | ax1.axvline(cutoff_freq, color='k') 128 | ax1.set_title('Magnitude Response') 129 | ax1.set_xlim(0, 0.5*fs) 130 | ax1.set_xlabel('Frequency (Hz)') 131 | ax1.set_ylabel('Magnitude') 132 | ax1.grid(True) 133 | ax1.legend(loc='best') 134 | 135 | angles = np.angle(h, deg=True) 136 | ax2.plot((fs * 0.5 / np.pi) * w, angles) 137 | ax2.plot(cutoff_freq, 0.5*np.sqrt(2), 'ko') 138 | ax2.axvline(cutoff_freq, color='k') 139 | ax2.set_title('Phase Response') 140 | ax2.set_xlim(0, 0.5*fs) 141 | ax2.set_ylim(-200.0, 200.0) 142 | ax2.set_xlabel('Frequency (Hz)') 143 | ax2.set_ylabel('Phase (deg)') 144 | ax2.grid(True) 145 | ax2.legend(loc='best') 146 | 147 | plt.tight_layout() 148 | 149 | if savefig: 150 | plt.savefig(plot_name+'.png', bbox_inches='tight') 151 | 152 | -------------------------------------------------------------------------------- /src/nodes/ApplyImpulse: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import logging, sys, os, time, subprocess, signal 3 | import rospy 4 | from bobble_controllers.msg import BobbleBotStatus 5 | from bobble_controllers.msg import ControlCommands 6 | from gazebo_msgs.srv import * 7 | from std_srvs.srv import Empty 8 | PKG = 'bobble_controllers' 9 | 10 | class ApplyImpulse(object): 11 | 12 | def __init__(self, impulse_force, data_output_file, run_length_s): 13 | """ 14 | This simple node applies an X impulse force to the simulated Bobble-Bot and 15 | logs the resulting motion. 16 | """ 17 | self.logger = logging.getLogger("sim_plots_test_log") 18 | self.impulse_force = impulse_force 19 | self.data_output_file = data_output_file 20 | self.run_length_s = run_length_s 21 | # Allow some time before registering services 22 | time.sleep(10.0) 23 | try: 24 | self.pause_physics_client = rospy.ServiceProxy('/gazebo/pause_physics', Empty) 25 | self.unpause_physics_client = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) 26 | self.reset_simulation = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) 27 | self.reset_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) 28 | self.apply_impulse_force = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench) 29 | self.set_link_properties = rospy.ServiceProxy('/gazebo/set_link_properties', SetLinkProperties) 30 | except rospy.ServiceException, e: 31 | self.logger.error("Service call failed: %s"%e) 32 | 33 | def resetState(self): 34 | control_cmd_publisher = rospy.Publisher('/bobble/bobble_balance_controller/bb_cmd', ControlCommands, queue_size=1) 35 | balance_cmd_msg = ControlCommands() 36 | balance_cmd_msg.IdleCmd = True 37 | balance_cmd_msg.StartupCmd = False 38 | balance_cmd_msg.DiagnosticCmd = False 39 | control_cmd_publisher.publish(balance_cmd_msg) 40 | return 41 | 42 | def activateController(self): 43 | self.logger.warn("Commanding controller active.") 44 | control_cmd_publisher = rospy.Publisher('/bobble/bobble_balance_controller/bb_cmd', ControlCommands, queue_size=1) 45 | balance_cmd_msg = ControlCommands() 46 | balance_cmd_msg.StartupCmd = True 47 | balance_cmd_msg.IdleCmd = False 48 | control_cmd_publisher.publish(balance_cmd_msg) 49 | 50 | def startDataRecord(self): 51 | self.rosbag_process = subprocess.Popen('rosbag record -e /bobble/bobble_balance_controller/bb_controller_status -j -o {} --duration {}'.format(self.data_output_file, "10.0"), stdin=subprocess.PIPE, shell=True) 52 | 53 | def applyImpulse(self): 54 | try: 55 | self.apply_impulse_force( 56 | body_name = "bobblebot::bobble_chassis_link", 57 | reference_frame = "bobblebot::bobble_chassis_link", 58 | reference_point = geometry_msgs.msg.Point(x=0.15, y=0.0, z=0.0), 59 | wrench = geometry_msgs.msg.Wrench(force = geometry_msgs.msg.Vector3(self.impulse_force, 0.0, 0.0), 60 | torque = geometry_msgs.msg.Vector3(0.0, 0.0, 0.0)), 61 | start_time = rospy.Time.from_sec(1.0), 62 | duration = rospy.Time.from_sec(0.001) 63 | ) 64 | except rospy.ServiceException, e: 65 | self.logger.error("Service call failed: %s"%e) 66 | 67 | def startDataRecord(self): 68 | self.rosbag_process = subprocess.Popen('rosbag record -e /bobble/bobble_balance_controller/bb_controller_status -j -O {} --duration {}'.format(self.data_output_file, "10.0"), stdin=subprocess.PIPE, shell=True) 69 | 70 | def run(self): 71 | self.logger.warn("Starting the sim dynamics.") 72 | self.unpause_physics_client() 73 | time.sleep(3.0) 74 | self.resetState() 75 | time.sleep(3.0) 76 | self.logger.warn("Activating controller.") 77 | self.activateController() 78 | time.sleep(0.25) 79 | self.logger.warn("Starting data recording.") 80 | self.startDataRecord() 81 | time.sleep(1.0) 82 | self.logger.warn("Applying impulse force in x direction of : " + str(self.impulse_force) + " Newtons.") 83 | self.applyImpulse() 84 | time.sleep(self.run_length_s) 85 | self.resetState() 86 | 87 | # Main function. 88 | if __name__ == '__main__': 89 | # Initialize the node and name it. 90 | log = logging.getLogger("sim_plots_test_log") 91 | log.setLevel(logging.INFO) 92 | rospy.init_node('ApplyImpulse', log_level=rospy.INFO) 93 | 94 | impulse_force = rospy.get_param('~impulse', "500.0") 95 | data_output_file = rospy.get_param('~data_output_file', "impulse_data") 96 | run_length_s = rospy.get_param('~run_length_s', "10.0") 97 | # Go to class functions that do all the heavy lifting. Do error checking. 98 | try: 99 | apply_impulse_sim = ApplyImpulse(impulse_force, data_output_file, run_length_s) 100 | apply_impulse_sim.run() 101 | except rospy.ROSInterruptException: 102 | pass 103 | rospy.signal_shutdown('Run complete.') 104 | -------------------------------------------------------------------------------- /src/nodes/BobbleStateHold: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | import logging 4 | from bobble_controllers.msg import BobbleBotStatus 5 | from gazebo_msgs.srv import * 6 | PKG = 'bobble_controllers' 7 | 8 | class BobbleStateHold(object): 9 | 10 | def __init__(self): 11 | """ 12 | This simple node just keeps the Bobble-Bot standing in sim until 13 | active control begins. 14 | """ 15 | self.logger = logging.getLogger("hold_state_log") 16 | self._ControlMode = None # Do not have a mode until the controller is actually running 17 | # Listen to bb_controller_status messages 18 | rospy.Subscriber("/bobble/bobble_balance_controller/bb_controller_status", BobbleBotStatus, self._controllerDataReceived) 19 | # Kluge to reset sim state until balance is active 20 | self.reset_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) 21 | # Create a timer to execute the control loop 22 | rospy.Timer(rospy.Duration(0.0025), self._mainLoop) 23 | # Allow time for initialization of all needed nodes 24 | # Allow ROS to go to all callbacks. 25 | rospy.spin() 26 | 27 | def _mainLoop(self, _event): 28 | # Reset sim state until we're in active control. We fall over other-wise 29 | if self._ControlMode == 0: 30 | self._resetSimState() 31 | 32 | def _resetSimState(self): 33 | try: 34 | self.reset_state(gazebo_msgs.msg.ModelState( 35 | model_name = "bobblebot", 36 | pose = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), 37 | orientation = geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)), 38 | twist = geometry_msgs.msg.Twist(linear = geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), 39 | angular = geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)), 40 | reference_frame = "world")) 41 | except Exception as e: 42 | self.logger.warning("Gazebo state was not able to be reset to zero.") 43 | 44 | return 45 | 46 | def _controllerDataReceived(self, controller_data): 47 | self._ControlMode = controller_data.ControlMode 48 | 49 | # Main function. 50 | if __name__ == '__main__': 51 | # Initialize the node and name it. 52 | rospy.init_node('BobbleStateHolder', log_level=rospy.INFO) 53 | # Go to class functions that do all the heavy lifting. Do error checking. 54 | try: 55 | bb_state_hold_sim = BobbleStateHold() 56 | except rospy.ROSInterruptException: 57 | pass 58 | -------------------------------------------------------------------------------- /src/nodes/JoystickControl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | from sensor_msgs.msg import Joy 4 | from geometry_msgs.msg import Twist 5 | from bobble_controllers.msg import ControlCommands 6 | PKG = 'bobble_controllers' 7 | 8 | FWD_VEL_AXIS = 1 9 | TURN_RATE_AXIS = 3 10 | #TURN_RATE_AXIS = 2 # Xbox One controller 11 | ACTIVATE_BUTTON = 0 12 | RESET_BUTTON = 4 13 | JOYSTICK_MIN = -1.0 14 | JOYSTICK_MAX = 1.0 15 | JOYSTICK_RANGE = JOYSTICK_MAX - JOYSTICK_MIN 16 | OUTPUT_MIN = -0.2 17 | OUTPUT_MAX = 0.2 18 | OUTPUT_RANGE = OUTPUT_MAX - OUTPUT_MIN 19 | 20 | 21 | class JoystickControl(object): 22 | 23 | def __init__(self): 24 | rospy.Subscriber('joy', Joy, self.joystickCallback) 25 | self.mode_cmd_pub = rospy.Publisher('/bobble/bobble_balance_controller/bb_cmd', ControlCommands, queue_size = 1) 26 | self.cmd_vel_pub = rospy.Publisher('/bobble/bobble_balance_controller/cmd_vel', Twist, queue_size = 1) 27 | self.mode_cmd = ControlCommands() 28 | self.cmd_vel = Twist() 29 | rospy.spin() 30 | 31 | def remap_joystick_range(self, axis_value): 32 | return (((axis_value - JOYSTICK_MIN) * OUTPUT_RANGE) / JOYSTICK_RANGE) + OUTPUT_MIN 33 | 34 | def joystickCallback(self, jsData): 35 | for axisNumber, axisValue in enumerate(jsData.axes): 36 | if axisNumber == FWD_VEL_AXIS: 37 | self.cmd_vel.linear.x = self.remap_joystick_range(axisValue) 38 | elif axisNumber == TURN_RATE_AXIS: 39 | self.cmd_vel.angular.z = self.remap_joystick_range(axisValue) 40 | for buttonNumber, buttonValue in enumerate(jsData.buttons): 41 | if buttonNumber == ACTIVATE_BUTTON and buttonValue: 42 | self.mode_cmd.IdleCmd = False 43 | self.mode_cmd.StartupCmd = True 44 | self.mode_cmd.DiagnosticCmd = False 45 | self.mode_cmd_pub.publish(self.mode_cmd) 46 | print('BobbleBot Active!') 47 | elif buttonNumber == RESET_BUTTON and buttonValue: 48 | self.mode_cmd.IdleCmd = True 49 | self.mode_cmd.StartupCmd = False 50 | self.mode_cmd.DiagnosticCmd = False 51 | self.mode_cmd_pub.publish(self.mode_cmd) 52 | print('Resetting BobbleBot') 53 | self.cmd_vel_pub.publish(self.cmd_vel) 54 | 55 | # Main function. 56 | if __name__ == '__main__': 57 | rospy.init_node('JoystickController', log_level=rospy.DEBUG) 58 | try: 59 | JoystickControl() 60 | except rospy.ROSInterruptException: 61 | pass 62 | -------------------------------------------------------------------------------- /src/nodes/KeyboardControl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from bobble_controllers.msg import ControlCommands 6 | import sys, select, termios, tty, time 7 | 8 | msg = """ 9 | BobbleBot Keyboard Controller 10 | --------------------------- 11 | Activate/Deactivate command: 12 | Activate/Shutdown: space bar 13 | Moving around: 14 | Forward : w 15 | Backward : s 16 | Left : a 17 | Right : d 18 | Speed Up/Down: 19 | 15% Increase: q 20 | 15% Decrease: e 21 | CTRL-C to quit 22 | """ 23 | 24 | moveBindings = { 25 | 'a':(0,1), 26 | 'w':(1,0), 27 | 's':(-1,0), 28 | 'd':(0,-1) 29 | } 30 | 31 | speedBindings={ 32 | 'q':(1.15,1.15), 33 | 'e':(0.85,0.85) 34 | } 35 | 36 | def getKey(): 37 | tty.setraw(sys.stdin.fileno()) 38 | select.select([sys.stdin], [], [], 0) 39 | key = sys.stdin.read(1) 40 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 41 | return key 42 | 43 | if __name__=="__main__": 44 | settings = termios.tcgetattr(sys.stdin) 45 | mode_cmd_pub = rospy.Publisher('/bobble/bobble_balance_controller/bb_cmd', ControlCommands, queue_size = 1) 46 | cmd_vel_pub = rospy.Publisher('/bobble/bobble_balance_controller/cmd_vel', Twist, queue_size = 1) 47 | rospy.init_node('KeyboardController') 48 | drive_fwd = 0 49 | turn_rate = 0 50 | status = 0 51 | fwd_speed = 0.25 52 | turn_speed = 0.25 53 | active = False 54 | mode_cmd = ControlCommands() 55 | 56 | try: 57 | print(msg) 58 | while(1): 59 | key = getKey() 60 | if key == ' ': 61 | if active: 62 | mode_cmd.IdleCmd = True 63 | mode_cmd.StartupCmd = False 64 | mode_cmd.DiagnosticCmd = False 65 | print('Reset state. Hit space bar again to resume.') 66 | mode_cmd_pub.publish(mode_cmd) 67 | active = False 68 | else: 69 | mode_cmd.IdleCmd = False 70 | mode_cmd.StartupCmd = True 71 | mode_cmd.DiagnosticCmd = False 72 | print('BobbleBot Active! Hit space bar again to shutdown.') 73 | mode_cmd_pub.publish(mode_cmd) 74 | active = True 75 | if key in moveBindings.keys(): 76 | drive_fwd = moveBindings[key][0] 77 | turn_rate = moveBindings[key][1] 78 | elif key in speedBindings.keys(): 79 | fwd_speed = fwd_speed * speedBindings[key][0] 80 | turn_speed = turn_speed * speedBindings[key][1] 81 | else: 82 | print('No key press. Zero vels') 83 | drive_fwd = 0.0 84 | turn_rate = 0.0 85 | if (key == '\x03'): 86 | break 87 | twist = Twist() 88 | twist.linear.x = drive_fwd*fwd_speed 89 | twist.angular.z = turn_rate*turn_speed 90 | print('Forward Velocity Cmd : ' + str(twist.linear.x)) 91 | print('Turn Rate Cmd : ' + str(twist.angular.z)) 92 | cmd_vel_pub.publish(twist) 93 | time.sleep(0.05) 94 | 95 | except Exception as e: 96 | print(e) 97 | 98 | finally: 99 | twist = Twist() 100 | twist.linear.x = 0 101 | twist.angular.z = 0 102 | cmd_vel_pub.publish(twist) 103 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 104 | -------------------------------------------------------------------------------- /src/nodes/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/src/nodes/__init__.py -------------------------------------------------------------------------------- /test/BobbleIntegrationTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import unittest, time, sys 3 | import rospy, rostest 4 | PKG = 'bobble_controllers' 5 | NAME = 'BobbleIntegrationTest' 6 | 7 | class BobbleIntegrationTest(unittest.TestCase): 8 | 9 | def setUp(self): 10 | run_length_s = rospy.get_param('~run_length_s', "30.0") 11 | time.sleep(run_length_s) 12 | 13 | def test_IntegrationTestResults(self): 14 | # For now there's nothing to do. The other nodes and rosbag play/record 15 | # do all the heavy lifting of the integration tests. This is just here 16 | # to let the CI know the test passed if it makes it this far w/o error. 17 | # In future, we could generate plots from in here. 18 | self.assertTrue(True) 19 | 20 | if __name__ == '__main__': 21 | rospy.init_node('BobbleIntegrationTest_Node', log_level=rospy.DEBUG) 22 | rostest.rosrun(PKG, NAME, BobbleIntegrationTest, sys.argv) 23 | 24 | -------------------------------------------------------------------------------- /test/bad_balance_control_config.yaml: -------------------------------------------------------------------------------- 1 | bobble: 2 | bobble_balance_controller: 3 | type: bobble_controllers/BalanceSimController 4 | joints: 5 | - right_wheel_hinge 6 | - left_wheel_hinge 7 | InSim: true 8 | ImuName: bno055 9 | ControlLoopFrequency: 500.0 10 | StartingTiltSafetyLimitDegrees: 10.0 11 | MaxTiltSafetyLimitDegrees: 20.0 12 | MotorEffortMax: 0.2 13 | ControllerEffortMax: 0.4 14 | MotorEffortToTorqueSimFactor: 1.0 15 | WheelVelocityAdjustment: 1.0 16 | MadgwickFilterGain: 0.015 17 | MeasuredTiltFilterFrequency: 100.0 18 | MeasuredTiltDotFilterFrequency: 100.0 19 | MeasuredHeadingFilterFrequency: 100.0 20 | MeasuredTurnRateFilterFrequency: 50.0 21 | LeftWheelVelocityFilterFrequency: 10.0 22 | RightWheelVelocityFilterFrequency: 10.0 23 | DesiredForwardVelocityFilterFrequency: 200.0 24 | DesiredTurnRateFilterFrequency: 50.0 25 | MaxVelocityCmd: 4.25 # 3 mph 26 | MaxTurnRateCmd: 6.25 # 150 deg/s 27 | WheelRadiusMeters: 0.05 28 | VelocityCmdScale: 1.15 29 | TurnCmdScale: 6.25 30 | VelocityControlKp: 0.125 31 | VelocityControlKd: 0.035 32 | VelocityControlDerivFilter: 5.0 33 | VelocityControlKi: 0.01 34 | VelocityControlAlphaFilter: 0.975 35 | VelocityControlMaxIntegralOutput: 0.15 36 | VelocityControlOutputLimitDegrees: 8.5 37 | TiltControlKp: 0.0 38 | TiltControlKd: 0.0 39 | TiltControlAlphaFilter: 0.05 40 | TiltOffset: 0.0 41 | TiltDotOffset: 0.0 42 | RollDotOffset: 0.0 43 | YawDotOffset: 0.0 44 | TurningControlKp: 0.015 45 | TurningControlKi: 0.001 46 | TurningControlKd: 0.0 47 | TurningControlDerivFilter: 50.0 48 | TurningOutputFilter: 0.5 49 | 50 | -------------------------------------------------------------------------------- /test/balance_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /test/balance_controller_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2018, S.O. Engineering, LLC 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of S.O. Engineering, LLC nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Mike Moore 29 | 30 | #include "test_common.h" 31 | 32 | // TEST CASES 33 | TEST_F(BalanceSimControllerTest, testBalance) 34 | { 35 | // wait for 3s. Is it holding balance? 36 | ros::Duration(3.0).sleep(); 37 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 38 | EXPECT_LT(fabs(status.Tilt), 5.0); // we should be standing 39 | } 40 | 41 | TEST_F(BalanceSimControllerTest, testForward) 42 | { 43 | // send a forward velocity command of 0.1 m/s 44 | geometry_msgs::Twist cmd_vel; 45 | cmd_vel.linear.x = 0.1; 46 | publish_vel_cmd(cmd_vel); 47 | // wait for 8s 48 | ros::Duration(8.0).sleep(); 49 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 50 | const double expected_velocity = 0.1; 51 | EXPECT_NEAR(status.ForwardVelocity, expected_velocity, VELOCITY_TOLERANCE); 52 | } 53 | 54 | TEST_F(BalanceSimControllerTest, testBackward) 55 | { 56 | // send a forward velocity command of -0.1 m/s 57 | geometry_msgs::Twist cmd_vel; 58 | cmd_vel.linear.x = -0.1; 59 | publish_vel_cmd(cmd_vel); 60 | // wait for 8s 61 | ros::Duration(8.0).sleep(); 62 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 63 | const double expected_velocity = -0.1; 64 | EXPECT_NEAR(status.ForwardVelocity, expected_velocity, VELOCITY_TOLERANCE); 65 | } 66 | 67 | TEST_F(BalanceSimControllerTest, testTurnLeft) 68 | { 69 | // send a turn rate command of 0.1 rad/s 70 | geometry_msgs::Twist cmd_vel; 71 | cmd_vel.angular.z = 0.125; 72 | publish_vel_cmd(cmd_vel); 73 | // wait for 3s 74 | ros::Duration(3.0).sleep(); 75 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 76 | const double expected_turn_rate = 25.0; 77 | EXPECT_GT(status.TurnRate, expected_turn_rate); // should be turning at least 25 deg/s 78 | } 79 | 80 | TEST_F(BalanceSimControllerTest, testTurnRight) 81 | { 82 | // send a turn rate command of -0.1 rad/s 83 | geometry_msgs::Twist cmd_vel; 84 | cmd_vel.angular.z = -0.125; 85 | publish_vel_cmd(cmd_vel); 86 | // wait for 3s 87 | ros::Duration(3.0).sleep(); 88 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 89 | const double expected_turn_rate = -25.0; 90 | EXPECT_LT(status.TurnRate, expected_turn_rate); // should be turning at least -25 deg/s 91 | } 92 | 93 | 94 | int main(int argc, char** argv) 95 | { 96 | testing::InitGoogleTest(&argc, argv); 97 | ros::init(argc, argv, "balance_controller_test"); 98 | 99 | ros::AsyncSpinner spinner(1); 100 | spinner.start(); 101 | int ret = RUN_ALL_TESTS(); 102 | spinner.stop(); 103 | ros::shutdown(); 104 | return ret; 105 | } 106 | -------------------------------------------------------------------------------- /test/common.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /test/data/balance.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/test/data/balance.bag -------------------------------------------------------------------------------- /test/data/drive_square_js_response.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/test/data/drive_square_js_response.bag -------------------------------------------------------------------------------- /test/data/impulse_force.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/test/data/impulse_force.bag -------------------------------------------------------------------------------- /test/data/no_balance.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/super-owesome/bobble_controllers/07ea4f9f1282d4e2cf5cc423aee7b5e0621a10c7/test/data/no_balance.bag -------------------------------------------------------------------------------- /test/filter_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2018, S.O. Engineering, LLC 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of S.O. Engineering, LLC nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author James Holley 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | // The fixture for testing class Foo. 37 | class FilterTest : public ::testing::Test { 38 | protected: 39 | // You can remove any or all of the following functions if their bodies would 40 | // be empty. 41 | 42 | FilterTest() { 43 | // You can do set-up work for each test here. 44 | } 45 | 46 | ~FilterTest() override { 47 | // You can do clean-up work that doesn't throw exceptions here. 48 | } 49 | 50 | // If the constructor and destructor are not enough for setting up 51 | // and cleaning up each test, you can define the following methods: 52 | 53 | void SetUp() override { 54 | // Code here will be called immediately after the constructor (right 55 | // before each test). 56 | } 57 | 58 | void TearDown() override { 59 | // Code here will be called immediately after each test (right 60 | // before the destructor). 61 | } 62 | 63 | // Class members declared here can be used by all tests in the test suite 64 | // for Foo. 65 | }; 66 | 67 | /// This test assumes a sample rate of 500, filter of 0.01 Hz, and damping of 0.707 68 | /// Input is a unit step, step results were generated from octave 69 | 70 | TEST_F(FilterTest, HPFTest) { 71 | HighPassFilter hpfFilter; 72 | const unsigned int RESPONSE_LENGTH = 26; 73 | const float ACCEPTABLE_ERROR = 1e-6; 74 | float response[RESPONSE_LENGTH] = { 75 | 0.5790339089, 76 | 0.0253449543, 77 | -0.1312539676, 78 | -0.1439021654, 79 | -0.1144519971, 80 | -0.0802678418, 81 | -0.0526085896, 82 | -0.0330492127, 83 | -0.0201670995, 84 | -0.0120484024, 85 | -0.0070829718, 86 | -0.0041114438, 87 | -0.0023622560, 88 | -0.0013458431, 89 | -0.0007613574, 90 | -0.0004281229, 91 | -0.0002394956, 92 | -0.0001333731, 93 | -0.0000739809, 94 | -0.0000408930, 95 | -0.0000225332, 96 | -0.0000123817, 97 | -0.0000067864, 98 | -0.0000037111, 99 | -0.0000020252, 100 | -0.0000011030, 101 | }; 102 | 103 | double filterResponse = 0.0; 104 | for(int i = 0; i < RESPONSE_LENGTH; i++) { 105 | filterResponse = hpfFilter.filter(1.0); 106 | ASSERT_NEAR(filterResponse, response[i], ACCEPTABLE_ERROR); 107 | } 108 | } 109 | 110 | TEST_F(FilterTest, LPFTest) { 111 | LowPassFilter lpfFilter; 112 | const unsigned int RESPONSE_LENGTH = 26; 113 | const float ACCEPTABLE_ERROR = 1e-6; 114 | float response[RESPONSE_LENGTH] = { 115 | 0.057148, 116 | 0.231095, 117 | 0.454238, 118 | 0.639772, 119 | 0.772651, 120 | 0.860813, 121 | 0.916642, 122 | 0.950903, 123 | 0.971458, 124 | 0.983581, 125 | 0.990636, 126 | 0.994698, 127 | 0.997016, 128 | 0.998330, 129 | 0.999069, 130 | 0.999484, 131 | 0.999714, 132 | 0.999843, 133 | 0.999913, 134 | 0.999953, 135 | 0.999974, 136 | 0.999986, 137 | 0.999992, 138 | 0.999996, 139 | 0.999998, 140 | 0.999999, 141 | }; 142 | 143 | double filterResponse = 0.0; 144 | for (int i = 0; i < RESPONSE_LENGTH; i++) { 145 | filterResponse = lpfFilter.filter(1.0); 146 | ASSERT_NEAR(filterResponse, response[i], ACCEPTABLE_ERROR); 147 | } 148 | } 149 | 150 | int main(int argc, char **argv) { 151 | ::testing::InitGoogleTest(&argc, argv); 152 | return RUN_ALL_TESTS(); 153 | } 154 | -------------------------------------------------------------------------------- /test/good_balance_control_config.yaml: -------------------------------------------------------------------------------- 1 | bobble: 2 | bobble_balance_controller: 3 | type: bobble_controllers/BalanceSimController 4 | joints: 5 | - right_wheel_hinge 6 | - left_wheel_hinge 7 | InSim: true 8 | ImuName: bno055 9 | ControlLoopFrequency: 500.0 10 | StartingTiltSafetyLimitDegrees: 10.0 11 | MaxTiltSafetyLimitDegrees: 20.0 12 | MotorEffortMax: 0.2 13 | ControllerEffortMax: 0.4 14 | MotorEffortToTorqueSimFactor: 1.0 15 | WheelVelocityAdjustment: 1.0 16 | MadgwickFilterGain: 0.015 17 | MeasuredTiltFilterFrequency: 100.0 18 | MeasuredTiltDotFilterFrequency: 100.0 19 | MeasuredHeadingFilterFrequency: 100.0 20 | MeasuredTurnRateFilterFrequency: 50.0 21 | LeftWheelVelocityFilterFrequency: 10.0 22 | RightWheelVelocityFilterFrequency: 10.0 23 | DesiredForwardVelocityFilterFrequency: 10.0 24 | DesiredTurnRateFilterFrequency: 50.0 25 | MaxVelocityCmd: 4.25 # 3 mph 26 | MaxTurnRateCmd: 6.25 # 150 deg/s 27 | WheelRadiusMeters: 0.05 28 | VelocityCmdScale: 1.15 29 | TurnCmdScale: 6.25 30 | VelocityControlKp: 0.125 31 | VelocityControlKd: 0.05 32 | VelocityControlDerivFilter: 20.0 33 | VelocityControlKi: 0.01 34 | VelocityControlAlphaFilter: 0.99 35 | VelocityControlMaxIntegralOutput: 0.025 36 | VelocityControlOutputLimitDegrees: 8.5 37 | TiltControlKp: 2.5 38 | TiltControlKd: 0.5 39 | TiltControlAlphaFilter: 0.05 40 | TiltOffset: 0.0 41 | TiltDotOffset: 0.0 42 | RollDotOffset: 0.0 43 | YawDotOffset: 0.0 44 | TurningControlKp: 0.25 45 | TurningControlKi: 0.05 46 | TurningControlKd: 0.0 47 | TurningControlDerivFilter: 50.0 48 | TurningOutputFilter: 0.5 49 | 50 | -------------------------------------------------------------------------------- /test/impulse_force.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /test/js_drive_square.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /test/nb_env.py: -------------------------------------------------------------------------------- 1 | # Establish the analysis environment used with Jupyter Notebook 2 | import os, sys 3 | import matplotlib 4 | matplotlib.use('agg') 5 | import matplotlib.pyplot as plt 6 | import warnings 7 | warnings.filterwarnings("ignore") 8 | import seaborn as sns 9 | import pandas as pd 10 | import numpy as np 11 | import scipy 12 | import scipy.fftpack 13 | import math 14 | try: 15 | from analysis_tools.parsing import * 16 | from analysis_tools.plots import * 17 | from analysis_tools.filters import * 18 | except ImportError: 19 | sys.path.append(os.path.join('..', 'src')) 20 | from analysis_tools.parsing import * 21 | from analysis_tools.plots import * 22 | from analysis_tools.filters import * 23 | 24 | # Set some global plotting configs. We want these for all plots 25 | sns.set_context("poster", font_scale=1.1) 26 | #sns.set_palette("Dark2") 27 | 28 | # Set paths to relevant data directories and config files 29 | data_home = os.path.join(os.getcwd(), "data") 30 | print("Reading all csv files in directory : ") 31 | print(data_home) 32 | 33 | # Load configs and data 34 | df = parse_all_csv_runs_in_dir(data_home) 35 | print("Successfully loaded runs : ") 36 | 37 | -------------------------------------------------------------------------------- /test/no_balance.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /test/no_balance_test.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2018, S.O. Engineering, LLC 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of S.O. Engineering, LLC nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Mike Moore 29 | 30 | #include "test_common.h" 31 | 32 | // TEST CASES 33 | TEST_F(BalanceSimControllerTest, testNoBalance) 34 | { 35 | // wait for 3s. Is it holding balance? 36 | ros::Duration(3.0).sleep(); 37 | bobble_controllers::BobbleBotStatus status = getLastStatus(); 38 | EXPECT_GT(fabs(status.Tilt), 5.0); // we should topple over 39 | } 40 | 41 | int main(int argc, char** argv) 42 | { 43 | testing::InitGoogleTest(&argc, argv); 44 | ros::init(argc, argv, "no_balance_test"); 45 | 46 | ros::AsyncSpinner spinner(1); 47 | spinner.start(); 48 | int ret = RUN_ALL_TESTS(); 49 | spinner.stop(); 50 | ros::shutdown(); 51 | return ret; 52 | } 53 | -------------------------------------------------------------------------------- /test/parsing_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import os 3 | 4 | import rospkg 5 | PKG="bobble_controllers" 6 | 7 | class ParsingTest(unittest.TestCase): 8 | 9 | def setUp(self): 10 | rospack = rospkg.RosPack() 11 | self.sample_data_path = os.path.join(rospack.get_path("bobble_controllers"), "test", "data") 12 | self.test_dir = os.path.join(rospack.get_path("bobble_controllers"), "test") 13 | self.run_name = 'no_balance' 14 | return 15 | 16 | def test_ParseConfigFile(self): 17 | from analysis_tools.parsing import parse_config_file 18 | plot_config = os.path.join(self.test_dir, 'plots.yaml') 19 | pc = parse_config_file(plot_config) 20 | self.assertEquals(pc['measured_tilt']['title'], 'Tilt Angle') 21 | return 22 | 23 | def test_ParseSingleBagFile(self): 24 | from analysis_tools.parsing import parse_single_run 25 | sim_run = os.path.join(self.sample_data_path, self.run_name+'.bag') 26 | df = parse_single_run(sim_run) 27 | final_tilt = df['Tilt'].iloc[-1] 28 | self.assertTrue(abs(final_tilt) > 35.0) 29 | return 30 | 31 | def test_ParseManyBagFiles(self): 32 | from analysis_tools.parsing import parse_all_runs_in_dir 33 | sim_data_dir = self.sample_data_path 34 | df = parse_all_runs_in_dir(sim_data_dir, csv_convert=True) 35 | final_tilt = df[self.run_name]['Tilt'].iloc[-1] 36 | self.assertTrue(abs(final_tilt) > 35.0) 37 | return 38 | 39 | def test_ParseManyCsvFiles(self): 40 | from analysis_tools.parsing import parse_all_csv_runs_in_dir 41 | sim_data_dir = self.sample_data_path 42 | df = parse_all_csv_runs_in_dir(sim_data_dir) 43 | final_tilt = df[self.run_name]['Tilt'].iloc[-1] 44 | self.assertTrue(abs(final_tilt) > 35.0) 45 | return 46 | 47 | if __name__ == '__main__': 48 | import rosunit 49 | rosunit.unitrun(PKG, 'parsings_test', ParsingTest) 50 | 51 | -------------------------------------------------------------------------------- /test/plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'Tilt' 4 | x_label: 'Time (s)' 5 | y_label: 'Tilt (deg)' 6 | runs : ['no_balance', 'impulse_force'] 7 | colors : ['grey', 'orange'] 8 | linestyles: ['-' , '-'] 9 | legend: 10 | no balance: 11 | color: 'grey' 12 | linestyle: '-' 13 | balanced: 14 | color: 'orange' 15 | linestyle: '-' 16 | xlim: [0.0, 10.0] 17 | 18 | velocity: 19 | title : 'Velocity' 20 | y_var: 'ForwardVelocity' 21 | x_label: 'Time (s)' 22 | y_label: 'Velocity (m/s)' 23 | runs : ['no_balance', 'impulse_force'] 24 | colors : ['grey', 'orange'] 25 | linestyles: ['-' , '-'] 26 | legend: 27 | no balance: 28 | color: 'grey' 29 | linestyle: '-' 30 | balanced: 31 | color: 'orange' 32 | linestyle: '-' 33 | xlim: [0.0, 10.0] 34 | 35 | tilt_control: 36 | title : 'Tilt Control' 37 | desired_y_var: 'DesiredTilt' 38 | actual_y_var: 'Tilt' 39 | runs : ['balance'] 40 | x_label: 'Time (s)' 41 | y_label: 'Tilt (deg)' 42 | colors : ['red', 'blue'] 43 | legend: 44 | desired: 45 | color: 'red' 46 | linestyle: '-' 47 | actual: 48 | color: 'blue' 49 | linestyle: '-' 50 | xlim: [0.0, 10.0] 51 | 52 | velocity_control: 53 | title : 'Velocity Control' 54 | desired_y_var: 'DesiredVelocity' 55 | actual_y_var: 'ForwardVelocity' 56 | runs : ['balance'] 57 | x_label: 'Time (s)' 58 | y_label: 'Velocity (m/s)' 59 | colors : ['red', 'blue'] 60 | legend: 61 | desired: 62 | color: 'red' 63 | linestyle: '-' 64 | actual: 65 | color: 'blue' 66 | linestyle: '-' 67 | xlim: [0.0, 10.0] 68 | 69 | turn_rate: 70 | title : 'Turn Rate - Robot Frame' 71 | y_var: 'TurnRate' 72 | runs : ['balance'] 73 | x_label: 'Time (s)' 74 | y_label: 'Turn Rate (deg/s)' 75 | colors : ['blue'] 76 | xlim: [0.0, 30.0] 77 | 78 | heading: 79 | title : 'Heading - World Frame' 80 | y_var: 'Heading' 81 | runs : ['balance'] 82 | x_label: 'Time (s)' 83 | y_label: 'Heading (deg)' 84 | colors : ['blue'] 85 | xlim: [0.0, 30.0] 86 | 87 | wheel_vel: 88 | title : 'Wheel Velocity' 89 | vars: ['LeftMotorVelocity', 90 | 'RightMotorVelocity'] 91 | runs : ['balance'] 92 | x_label: 'Time (s)' 93 | y_label: 'Velocity (deg/s)' 94 | colors : ['orange', 'grey'] 95 | legend: 96 | left: 97 | color: 'orange' 98 | linestyle: '-' 99 | right: 100 | color: 'grey' 101 | linestyle: '-' 102 | xlim: [0.0, 30.0] 103 | 104 | loop_jitter: 105 | title : 'Loop Jitter' 106 | y_var: 'DeltaT' 107 | runs : ['balance'] 108 | x_label: 'Time (s)' 109 | y_label: 'Delta-T (s)' 110 | colors : ['blue'] 111 | xlim: [0.0, 30.0] 112 | 113 | turning_control: 114 | title : 'Turning Control' 115 | desired_y_var: 'DesiredTurnRate' 116 | actual_y_var: 'TurnRate' 117 | runs : ['balance'] 118 | x_label: 'Time (s)' 119 | y_label: 'Turn Rate (deg/s)' 120 | colors : ['red', 'blue'] 121 | legend: 122 | desired: 123 | color: 'red' 124 | linestyle: '-' 125 | actual: 126 | color: 'blue' 127 | linestyle: '-' 128 | xlim: [0.0, 30.0] 129 | 130 | heading_derived: 131 | title : 'Derived Heading - World Frame' 132 | y_var: 'DerivedHeading' 133 | runs : ['balance'] 134 | x_label: 'Time (s)' 135 | y_label: 'Heading (deg)' 136 | colors : ['blue'] 137 | xlim: [0.0, 30.0] 138 | 139 | position_derived: 140 | title : 'Derived Position - World Frame' 141 | x_var: 'DerivedXPos' 142 | y_var: 'DerivedYPos' 143 | runs : ['balance'] 144 | x_label: 'Pos-X (m)' 145 | y_label: 'Pos-Y (m)' 146 | colors : ['blue'] 147 | xlim: [-1.0, 2.0] 148 | ylim: [-1.0, 2.0] 149 | 150 | heading_control: 151 | title : 'Heading Control - World Frame' 152 | desired_y_var: 'DesiredHeading' 153 | actual_y_var: 'DerivedHeading' 154 | runs : ['balance'] 155 | x_label: 'Time (s)' 156 | y_label: 'Heading (deg)' 157 | colors : ['red', 'blue'] 158 | legend: 159 | desired: 160 | color: 'red' 161 | linestyle: '-' 162 | actual: 163 | color: 'blue' 164 | linestyle: '-' 165 | xlim: [0.0, 30.0] 166 | 167 | position_control: 168 | title : 'Position Control - World Frame' 169 | desired_x_var: 'DesiredXPos' 170 | desired_y_var: 'DesiredYPos' 171 | actual_x_var: 'DerivedXPos' 172 | actual_y_var: 'DerivedYPos' 173 | runs : ['balance'] 174 | x_label: 'Pos-X (m)' 175 | y_label: 'Pos-Y (m)' 176 | colors : ['red', 'blue'] 177 | legend: 178 | desired: 179 | color: 'red' 180 | linestyle: '-' 181 | actual: 182 | color: 'blue' 183 | linestyle: '-' 184 | xlim: [-1.0, 2.0] 185 | ylim: [-1.0, 2.0] 186 | 187 | -------------------------------------------------------------------------------- /test/plots_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import os 3 | import matplotlib 4 | matplotlib.use('agg') 5 | import matplotlib.pyplot as plt 6 | import seaborn as sns 7 | # Set some global plotting configs. We want these for all plots 8 | plt.rcParams['axes.grid'] = True 9 | sns.set_context("poster", font_scale=1.1) 10 | sns.set_palette("Dark2") 11 | 12 | import rospkg 13 | PKG="bobble_controllers" 14 | 15 | class PlotsTest(unittest.TestCase): 16 | 17 | def setUp(self): 18 | rospack = rospkg.RosPack() 19 | self.sample_data_path = os.path.join(rospack.get_path("bobble_controllers"), "test", "data") 20 | self.test_dir = os.path.join(rospack.get_path("bobble_controllers"), "test") 21 | return 22 | 23 | def helper_load_data(self): 24 | from analysis_tools.parsing import parse_config_file, parse_all_csv_runs_in_dir 25 | # Parse configs 26 | plot_config = os.path.join(self.test_dir, 'plots.yaml') 27 | pc = parse_config_file(plot_config) 28 | # Parse data 29 | sim_runs_data_path = self.sample_data_path 30 | return parse_all_csv_runs_in_dir(sim_runs_data_path), pc 31 | 32 | def test_PlotManyRunsTilt(self): 33 | from analysis_tools.plots import coplot_var_for_runs 34 | df, pc = self.helper_load_data() 35 | # Make the plot 36 | fig = plt.figure(figsize=(20, 10), dpi=40) 37 | ax1 = fig.add_subplot(111) 38 | coplot_var_for_runs(ax1, df, pc['measured_tilt']) 39 | fig.tight_layout() 40 | fig_file_name = os.path.join(self.test_dir, 'MeasuredTilt.png') 41 | plt.savefig(fig_file_name, bbox_inches='tight') 42 | 43 | def test_PlotManyRunsVelocity(self): 44 | from analysis_tools.plots import coplot_var_for_runs 45 | df, pc = self.helper_load_data() 46 | # Make the plot 47 | fig = plt.figure(figsize=(20, 10), dpi=40) 48 | ax1 = fig.add_subplot(111) 49 | coplot_var_for_runs(ax1, df, pc['velocity']) 50 | fig.tight_layout() 51 | fig_file_name = os.path.join(self.test_dir, 'MeasuredVelocity.png') 52 | plt.savefig(fig_file_name, bbox_inches='tight') 53 | 54 | def test_PlotDesiredVsActual(self): 55 | from analysis_tools.plots import desired_vs_actual_for_runs 56 | df, pc = self.helper_load_data() 57 | # Make the Tilt plot 58 | fig = plt.figure(figsize=(20, 10), dpi=40) 59 | ax1 = fig.add_subplot(111) 60 | desired_vs_actual_for_runs(ax1, df, pc['tilt_control']) 61 | fig.tight_layout() 62 | fig_file_name = os.path.join(self.test_dir, 'TiltControl.png') 63 | plt.savefig(fig_file_name, bbox_inches='tight') 64 | # Make the Velocity plot 65 | fig = plt.figure(figsize=(20, 10), dpi=40) 66 | ax1 = fig.add_subplot(111) 67 | desired_vs_actual_for_runs(ax1, df, pc['velocity_control']) 68 | fig.tight_layout() 69 | fig_file_name = os.path.join(self.test_dir, 'VelocityControl.png') 70 | plt.savefig(fig_file_name, bbox_inches='tight') 71 | 72 | 73 | if __name__ == '__main__': 74 | import rosunit 75 | rosunit.unitrun(PKG, 'plots_test', PlotsTest) 76 | 77 | -------------------------------------------------------------------------------- /test/test_common.h: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2018, S.O. Engineering, LLC 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of S.O. Engineering, LLC nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | /// \author Mike Moore 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | // Floating-point value comparison threshold 38 | const double VELOCITY_TOLERANCE = 0.05; // 5 cm-s precision 39 | 40 | class BalanceSimControllerTest : public ::testing::Test 41 | { 42 | public: 43 | 44 | BalanceSimControllerTest() 45 | : mode_cmd_pub(nh.advertise("/bobble/bobble_balance_controller/bb_cmd", 1)), 46 | cmd_vel_pub(nh.advertise("/bobble/bobble_balance_controller/cmd_vel", 1)), 47 | bobble_status_sub(nh.subscribe("/bobble/bobble_balance_controller/bb_controller_status", 1, &BalanceSimControllerTest::statusCallback, this)) 48 | { 49 | } 50 | 51 | ~BalanceSimControllerTest() 52 | { 53 | bobble_status_sub.shutdown(); 54 | } 55 | 56 | void SetUp(){ 57 | waitForController(); 58 | ros::Duration(0.25).sleep(); 59 | ROS_INFO("Activating balance controller."); 60 | ros::Duration(0.25).sleep(); 61 | activate(); 62 | } 63 | 64 | void publish_vel_cmd(geometry_msgs::Twist cmd_vel){ cmd_vel_pub.publish(cmd_vel); } 65 | 66 | bobble_controllers::BobbleBotStatus getLastStatus(){ return last_status; } 67 | 68 | void activate() 69 | { 70 | bobble_controllers::ControlCommands mode_cmd; 71 | mode_cmd.StartupCmd = true; 72 | mode_cmd.IdleCmd = false; 73 | mode_cmd.DiagnosticCmd = false; 74 | mode_cmd_pub.publish(mode_cmd); 75 | } 76 | 77 | void reset() 78 | { 79 | geometry_msgs::Twist cmd_vel; 80 | cmd_vel.linear.x = 0.0; 81 | cmd_vel.angular.z = 0.0; 82 | publish_vel_cmd(cmd_vel); 83 | ros::Duration(0.25).sleep(); 84 | bobble_controllers::ControlCommands mode_cmd; 85 | mode_cmd.IdleCmd = true; 86 | mode_cmd.StartupCmd = false; 87 | mode_cmd.DiagnosticCmd = false; 88 | mode_cmd_pub.publish(mode_cmd); 89 | ros::Duration(0.25).sleep(); 90 | } 91 | 92 | bool isControllerAlive()const{ return (bobble_status_sub.getNumPublishers() > 0) && (cmd_vel_pub.getNumSubscribers() > 0); } 93 | 94 | void waitForController() const 95 | { 96 | while(!isControllerAlive() && ros::ok()) 97 | { 98 | ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller."); 99 | ros::Duration(0.1).sleep(); 100 | } 101 | if (!ros::ok()) 102 | FAIL() << "Something went wrong while executing test."; 103 | } 104 | 105 | void statusCallback(const bobble_controllers::BobbleBotStatus& status) 106 | { 107 | ROS_INFO_STREAM("Callback received: status.ForwardVelocity: " << status.ForwardVelocity 108 | << ", status.Tilt: " << status.Tilt); 109 | last_status = status; 110 | received_first_status = true; 111 | } 112 | 113 | bool hasReceivedFirstStatus()const{ return received_first_status; } 114 | 115 | void waitForStatusMsgs() const 116 | { 117 | while(!hasReceivedFirstStatus() && ros::ok()) 118 | { 119 | ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for status messages to be published."); 120 | ros::Duration(0.01).sleep(); 121 | } 122 | if (!ros::ok()) 123 | FAIL() << "Something went wrong while executing test."; 124 | } 125 | 126 | private: 127 | bool received_first_status; 128 | ros::NodeHandle nh; 129 | ros::Publisher mode_cmd_pub; 130 | ros::Publisher cmd_vel_pub; 131 | ros::Subscriber bobble_status_sub; 132 | bobble_controllers::BobbleBotStatus last_status; 133 | }; 134 | 135 | -------------------------------------------------------------------------------- /test/test_summary_plots.yaml: -------------------------------------------------------------------------------- 1 | measured_tilt: 2 | title : 'Tilt Angle' 3 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 4 | runs : ['response'] 5 | x_label: 'Time (s)' 6 | y_label: 'Tilt (deg)' 7 | colors : ['blue'] 8 | xlim: [0.0, 20.0] 9 | 10 | velocity: 11 | title : 'Velocity - Robot Frame' 12 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 13 | runs : ['response'] 14 | x_label: 'Time (s)' 15 | y_label: 'Velocity (m/s)' 16 | colors : ['blue'] 17 | xlim: [0.0, 20.0] 18 | 19 | turn_rate: 20 | title : 'Turn Rate - Robot Frame' 21 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 22 | runs : ['response'] 23 | x_label: 'Time (s)' 24 | y_label: 'Turn Rate (deg/s)' 25 | colors : ['blue'] 26 | xlim: [0.0, 20.0] 27 | 28 | heading: 29 | title : 'Heading - World Frame' 30 | y_var: 'bobble_bobble_balance_controller_bb_controller_status__Heading' 31 | runs : ['response'] 32 | x_label: 'Time (s)' 33 | y_label: 'Heading (deg)' 34 | colors : ['blue'] 35 | xlim: [0.0, 20.0] 36 | 37 | wheel_vel: 38 | title : 'Wheel Velocity' 39 | vars: ['bobble_bobble_balance_controller_bb_controller_status__LeftMotorVelocity', 40 | 'bobble_bobble_balance_controller_bb_controller_status__RightMotorVelocity'] 41 | run : 'response' 42 | x_label: 'Time (s)' 43 | y_label: 'Velocity (deg/s)' 44 | colors : ['orange', 'grey'] 45 | legend: 46 | left: 47 | color: 'orange' 48 | linestyle: '-' 49 | right: 50 | color: 'grey' 51 | linestyle: '-' 52 | xlim: [0.0, 20.0] 53 | 54 | loop_jitter: 55 | title : 'Loop Jitter' 56 | y_var: 'bobble_bobble_bot_control_node_rt_status__DeltaT' 57 | runs : ['timing'] 58 | x_label: 'Time (s)' 59 | y_label: 'Delta-T (s)' 60 | colors : ['blue'] 61 | xlim: [0.0, 20.0] 62 | 63 | tilt_control: 64 | title : 'Tilt Control' 65 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTilt' 66 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__Tilt' 67 | runs : ['response'] 68 | x_label: 'Time (s)' 69 | y_label: 'Tilt (deg)' 70 | colors : ['red', 'blue'] 71 | legend: 72 | desired: 73 | color: 'red' 74 | linestyle: '-' 75 | actual: 76 | color: 'blue' 77 | linestyle: '-' 78 | xlim: [0.0, 20.0] 79 | 80 | velocity_control: 81 | title : 'Velocity Control' 82 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredVelocity' 83 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__ForwardVelocity' 84 | runs : ['response'] 85 | x_label: 'Time (s)' 86 | y_label: 'Velocity (m/s)' 87 | cmd_data : 'cmds' 88 | resp_data : 'response' 89 | colors : ['red', 'blue'] 90 | legend: 91 | desired: 92 | color: 'red' 93 | linestyle: '-' 94 | actual: 95 | color: 'blue' 96 | linestyle: '-' 97 | xlim: [0.0, 20.0] 98 | 99 | turning_control: 100 | title : 'Turning Control' 101 | desired_y_var: 'bobble_bobble_balance_controller_bb_controller_status__DesiredTurnRate' 102 | actual_y_var: 'bobble_bobble_balance_controller_bb_controller_status__TurnRate' 103 | runs : ['response'] 104 | x_label: 'Time (s)' 105 | y_label: 'Turn Rate (deg/s)' 106 | cmd_data : 'cmds' 107 | resp_data : 'response' 108 | colors : ['red', 'blue'] 109 | legend: 110 | desired: 111 | color: 'red' 112 | linestyle: '-' 113 | actual: 114 | color: 'blue' 115 | linestyle: '-' 116 | xlim: [0.0, 20.0] 117 | 118 | heading_derived: 119 | title : 'Derived Heading - World Frame' 120 | y_var: 'DerivedHeading' 121 | runs : ['response'] 122 | x_label: 'Time (s)' 123 | y_label: 'Heading (deg)' 124 | colors : ['blue'] 125 | xlim: [0.0, 20.0] 126 | 127 | position_derived: 128 | title : 'Derived Position - World Frame' 129 | x_var: 'DerivedXPos' 130 | y_var: 'DerivedYPos' 131 | runs : ['response'] 132 | x_label: 'Pos-X (m)' 133 | y_label: 'Pos-Y (m)' 134 | colors : ['blue'] 135 | xlim: [-1.0, 2.0] 136 | ylim: [-1.0, 2.0] 137 | 138 | heading_control: 139 | title : 'Heading Control - World Frame' 140 | desired_y_var: 'DesiredHeading' 141 | actual_y_var: 'DerivedHeading' 142 | runs : ['response'] 143 | x_label: 'Time (s)' 144 | y_label: 'Heading (deg)' 145 | colors : ['red', 'blue'] 146 | legend: 147 | desired: 148 | color: 'red' 149 | linestyle: '-' 150 | actual: 151 | color: 'blue' 152 | linestyle: '-' 153 | xlim: [0.0, 20.0] 154 | 155 | position_control: 156 | title : 'Position Control - World Frame' 157 | desired_x_var: 'DesiredXPos' 158 | desired_y_var: 'DesiredYPos' 159 | actual_x_var: 'DerivedXPos' 160 | actual_y_var: 'DerivedYPos' 161 | runs : ['response'] 162 | x_label: 'Pos-X (m)' 163 | y_label: 'Pos-Y (m)' 164 | colors : ['red', 'blue'] 165 | legend: 166 | desired: 167 | color: 'red' 168 | linestyle: '-' 169 | actual: 170 | color: 'blue' 171 | linestyle: '-' 172 | xlim: [-1.0, 2.0] 173 | ylim: [-1.0, 2.0] 174 | 175 | position_compare: 176 | title: 'Position - Hw vs Sim' 177 | x_var: 'DerivedXPos' 178 | y_var: 'DerivedYPos' 179 | runs : ['response', 'response_sim'] 180 | x_label: 'Pos-X (m)' 181 | y_label: 'Pos-Y (m)' 182 | colors : ['blue', 'red'] 183 | linestyles: ['-' , '--'] 184 | legend: 185 | hw: 186 | color: 'blue' 187 | linestyle: '-' 188 | sim: 189 | color: 'red' 190 | linestyle: '--' 191 | xlim: [-1.0, 2.0] 192 | ylim: [-1.0, 2.0] -------------------------------------------------------------------------------- /test/xacro/bno055_imu.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | false 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /test/xacro/bobble.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | /bobble 22 | gazebo_ros_control/DefaultRobotHWSim 23 | 0.002 24 | false 25 | 26 | 27 | true 28 | 200.0 29 | bno055_imu_link 30 | /imu_bosch/data_raw 31 | 0.02 32 | 0 0 0 33 | 0 0 0 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /test/xacro/bobble_chassis.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | false 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /test/xacro/bobble_world.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /test/xacro/left_wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | false 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | transmission_interface/SimpleTransmission" 43 | 44 | hardware_interface/EffortJointInterface 45 | 46 | 47 | hardware_interface/EffortJointInterface 48 | 1 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /test/xacro/right_wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | false 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | transmission_interface/SimpleTransmission" 43 | 44 | hardware_interface/EffortJointInterface 45 | 46 | 47 | hardware_interface/EffortJointInterface 48 | 1 49 | 50 | 51 | 52 | 53 | 54 | 55 | --------------------------------------------------------------------------------