├── .clang-format ├── .codespellignore ├── .devcontainer ├── Dockerfile └── devcontainer.json ├── .gitignore ├── .pre-commit-config.yaml ├── README.md ├── img ├── cart_pole.png └── cart_pole_system.png └── src └── cart_pole ├── cart_pole_bringup ├── CMakeLists.txt ├── config │ └── gazebo_bridge.yaml ├── hooks │ └── cart_pole_bringup.sh.in ├── launch │ └── cart_pole.launch.py ├── meshes │ ├── cart.dae │ ├── pole_with_holder.dae │ └── slide_bar.dae ├── package.xml └── urdf │ ├── cart_pole.urdf │ ├── cart_pole.urdf.xacro │ └── macros.urdf.xacro ├── cart_pole_model_based_controller ├── LICENSE ├── cart_pole_model_based_controller │ ├── __init__.py │ ├── cart_pole_system.ipynb │ └── linear_approximation_control_node.py ├── package.xml ├── resource │ └── cart_pole_model_based_controller ├── setup.cfg └── setup.py ├── cart_pole_observation ├── CMakeLists.txt ├── package.xml └── src │ └── cart_pole_observation_node.cpp ├── cart_pole_observation_interface ├── CMakeLists.txt ├── LICENSE ├── msg │ └── CartPoleObservation.msg └── package.xml ├── cart_pole_reinforcement_learning ├── LICENSE ├── cart_pole_reinforcement_learning │ ├── __init__.py │ ├── cart_pole_basic_policy_node.py │ ├── cart_pole_deep_q_learning_policy_node.py │ ├── cart_pole_learning_control_node.py │ ├── cart_pole_neural_network_policy.py │ ├── reinforcement_learning_node.py │ └── reinforcement_learning_node_parameters.yaml ├── package.xml ├── resource │ └── cart_pole_reinforcement_learning ├── setup.cfg └── setup.py ├── genesis_simulations ├── LICENSE ├── genesis_simulations │ ├── __init__.py │ └── cart_pole_basic_policy.py ├── package.xml ├── resource │ └── genesis_simulations ├── setup.cfg └── setup.py └── simulation_control ├── CMakeLists.txt ├── LICENSE ├── package.xml └── src └── simulation_control_node.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | 4 | ColumnLimit: 120 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: AlwaysBreak 7 | BreakBeforeBraces: Allman 8 | ConstructorInitializerIndentWidth: 0 9 | ContinuationIndentWidth: 2 10 | DerivePointerAlignment: false 11 | PointerAlignment: Left 12 | ReflowComments: true 13 | IncludeBlocks: Preserve 14 | -------------------------------------------------------------------------------- /.codespellignore: -------------------------------------------------------------------------------- 1 | ARENT 2 | Arent 3 | -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=jazzy 2 | FROM althack/ros2:$ROS_DISTRO-full 3 | ARG USERNAME=ros 4 | ARG USER_UID=1000 5 | ARG USER_GID=$USER_UID 6 | 7 | ENV SHELL /bin/bash 8 | ENV DEBIAN_FRONTEND=noninteractive 9 | RUN echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ 10 | && chmod 0440 /etc/sudoers.d/$USERNAME 11 | 12 | RUN apt-get update && apt-get upgrade -y \ 13 | && apt-get install -y wget bash-completion git-core pre-commit 14 | 15 | RUN sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \ 16 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null && \ 17 | apt-get update --fix-missing && apt-get -y install \ 18 | ros-$ROS_DISTRO-ros-gz \ 19 | ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ 20 | ros-$ROS_DISTRO-generate-parameter-library-py \ 21 | && apt-get autoremove -y \ 22 | && apt-get clean \ 23 | && rm -rf /var/lib/apt/lists/* 24 | 25 | RUN python3 -m pip install tensorflow==2.17.0 --break-system-packages 26 | RUN python3 -m pip install control sympy scipy ipykernel nbconvert==7.16.1 nbformat --break-system-packages 27 | RUN python3 -m pip install torch --break-system-packages 28 | RUN git clone https://github.com/Genesis-Embodied-AI/Genesis.git && \ 29 | cd Genesis && pip install --break-system-packages -e . 30 | ENV DEBIAN_FRONTEND=dialog 31 | 32 | ARG WORKSPACE 33 | RUN echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ 34 | && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ 35 | && echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> //home/$USERNAME/.bashrc 36 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "dockerFile": "Dockerfile", 3 | "build": { 4 | "args": { 5 | "WORKSPACE": "${containerWorkspaceFolder}" 6 | } 7 | }, 8 | "remoteUser": "ros", 9 | "runArgs": [ 10 | "--network=host", 11 | "--cap-add=SYS_PTRACE", 12 | "--security-opt=seccomp:unconfined", 13 | "--security-opt=apparmor:unconfined", 14 | "--ipc=host", 15 | "--runtime=nvidia", 16 | "--device=/dev/dri" 17 | ], 18 | "mounts": [ 19 | "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached" 20 | ], 21 | "containerEnv": { 22 | "DISPLAY": "${localEnv:DISPLAY}", 23 | "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", 24 | "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", 25 | "PULSE_SERVER": "${localEnv:PULSE_SERVER}", 26 | "LIBGL_ALWAYS_SOFTWARE": "1", 27 | "NVIDIA_DRIVER_CAPABILITIES": "all", 28 | "NVIDIA_VISIBLE_DEVICES": "all", 29 | "QT_X11_NO_MITSHM": "1", 30 | "PYOPENGL_PLATFORM": "glx", 31 | "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp", 32 | }, 33 | "customizations": { 34 | "vscode": { 35 | "extensions": [ 36 | "ms-azuretools.vscode-docker", 37 | "ms-python.python", 38 | "ms-vscode.cpptools", 39 | "streetsidesoftware.code-spell-checker", 40 | "ms-vscode.cmake-tools", 41 | "ms-toolsai.jupyter" 42 | ] 43 | } 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | install/ 3 | log/ 4 | .vscode/ 5 | notes.md 6 | __pycache__/ 7 | cart_pole_reinforcement_learning_params.py 8 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/mirrors-clang-format 3 | rev: v19.1.3 4 | hooks: 5 | - id: clang-format 6 | exclude_types: [json] 7 | 8 | - repo: https://github.com/pre-commit/pre-commit-hooks 9 | rev: v4.5.0 10 | hooks: 11 | - id: check-merge-conflict 12 | - id: trailing-whitespace 13 | - id: end-of-file-fixer 14 | - id: check-yaml 15 | - id: check-xml 16 | - id: check-ast 17 | 18 | - repo: https://github.com/codespell-project/codespell 19 | rev: v2.3.0 20 | hooks: 21 | - id: codespell 22 | name: codespell 23 | description: Checks for common misspellings in text files. 24 | language: python 25 | types: [text] 26 | args: ["--skip=*.ipynb", "-L Arent, ARENT"] 27 | 28 | - repo: https://github.com/psf/black 29 | rev: 24.4.2 30 | hooks: 31 | - id: black 32 | args: ["--line-length=120", "--preview"] 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Reinforcement learning playground 2 | 3 | Currently repository allows to play with cart pole model. 4 | 5 | [![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) 6 | 7 | ### Cart pole simulation 8 | ![cart pole](/img/cart_pole.png " ") 9 | 10 | ### Model of the cart pole 11 | 12 | The model of the cart pole was initially created and analyzed in publication: 13 | 14 | * > K. Arent, M. Szulc, 15 | > **"Experimental stand with a pendulum on a cart: construction, modelling, identification, model based control and deployment"** 16 | 17 | @chapter {ARENT2018 18 | title = "Experimental stand with a pendulum on a cart: construction, modelling, identification, model based control and deployment" 19 | authors = "KRZYSZTOF ARENT and Mateusz Szulc" 20 | booktitle = "Postępy robotyki" 21 | year = "2018" 22 | pages = "365-376" 23 | } 24 | 25 | The cart pole is located in [The Laboratory of Robotics](https://lr.kcir.pwr.edu.pl/) on [Wrocaław University of Science and Technology](https://pwr.edu.pl/). 26 | 27 | 28 | 29 | ### Run simulation 30 | 31 | Open folder in a devcontainer. Then type 32 | 33 | ```bash 34 | colcon build --symlink-install 35 | source install/setup.bash 36 | ros2 launch cart_pole_bringup cart_pole.launch.py 37 | ``` 38 | 39 | ### Linear approximation in an unstable equilibrium point 40 | 41 | A model of an inverted pendulum on a cart and controller are derived in the `cart_pole_system.ipynb`. 42 | 43 | The model was applied to control the cart pole in the script `linear_approximation_control_node.py`. 44 | 45 | ```bash 46 | source install/setup.bash 47 | ros2 run cart_pole_model_based_controller linear_approximation_control_node 48 | ``` 49 | 50 | ### Reinforcement learning nodes 51 | 52 | Repository currently contains three reinforcement learning policies: 53 | * Basic policy 54 | * Neural network policy 55 | * Deep q learning policy 56 | 57 | All implementations are based on the implementations presented in the `Hands-On Machine Learning with Scikit-Learn, Keras, and Tensorflow: Concepts, Tools, and Techniques to Build Intelligent` 58 | 59 | ### Launching 60 | To run simulations use command given above and in other terminal use one of following command: 61 | 62 | ```bash 63 | source install/setup.bash 64 | ros2 run cart_pole_reinforcement_learning cart_pole_basic_policy_node 65 | ``` 66 | 67 | or 68 | 69 | ```bash 70 | source install/setup.bash 71 | ros2 run cart_pole_reinforcement_learning cart_pole_neural_network_policy 72 | ``` 73 | 74 | or 75 | 76 | ```bash 77 | source install/setup.bash 78 | ros2 run cart_pole_reinforcement_learning cart_pole_deep_q_learning_policy_node 79 | ``` 80 | 81 | DQN model works poorly for now. 82 | 83 | Rolling branch uses cuda-image in the devcontainer, non cuda version is on the main branch. 84 | -------------------------------------------------------------------------------- /img/cart_pole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/img/cart_pole.png -------------------------------------------------------------------------------- /img/cart_pole_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/img/cart_pole_system.png -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cart_pole_bringup) 3 | 4 | 5 | find_package(ament_cmake REQUIRED) 6 | 7 | ament_environment_hooks( 8 | "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") 9 | 10 | install(DIRECTORY 11 | launch 12 | config 13 | meshes 14 | urdf 15 | DESTINATION share/${PROJECT_NAME}/ 16 | ) 17 | 18 | ament_package() 19 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/config/gazebo_bridge.yaml: -------------------------------------------------------------------------------- 1 | - ros_topic_name: "clock" 2 | gz_topic_name: "clock" 3 | ros_type_name: "rosgraph_msgs/msg/Clock" 4 | gz_type_name: "gz.msgs.Clock" 5 | direction: GZ_TO_ROS 6 | 7 | - ros_topic_name: "effort_cmd" 8 | gz_topic_name: "effort_cmd" 9 | ros_type_name: "std_msgs/msg/Float64" 10 | gz_type_name: "gz.msgs.Double" 11 | direction: ROS_TO_GZ 12 | 13 | - ros_topic_name: "joint_states" 14 | gz_topic_name: "joint_states" 15 | ros_type_name: "sensor_msgs/msg/JointState" 16 | gz_type_name: "gz.msgs.Model" 17 | direction: GZ_TO_ROS -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/hooks/cart_pole_bringup.sh.in: -------------------------------------------------------------------------------- 1 | ament_prepend_unique_value GAZEBO_MODEL_PATH "@CMAKE_INSTALL_PREFIX@/share" 2 | ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" 3 | ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" 4 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/launch/cart_pole.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch_ros.actions import Node 3 | from launch import LaunchDescription 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.substitutions import Command 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | 9 | def generate_launch_description(): 10 | gazebo = IncludeLaunchDescription( 11 | os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"), 12 | launch_arguments=[("gz_args", ["-r -v 4 empty.sdf"])], 13 | ) 14 | 15 | gazebo_spawn_robot = Node( 16 | package="ros_gz_sim", 17 | executable="create", 18 | name="spawn_cart_pole", 19 | arguments=["-name", "cart_pole", "-topic", "robot_description"], 20 | output="screen", 21 | ) 22 | 23 | xacro_file = os.path.join(get_package_share_directory("cart_pole_bringup"), "urdf", "cart_pole.urdf.xacro") 24 | robot_state_publisher = Node( 25 | package="robot_state_publisher", 26 | executable="robot_state_publisher", 27 | output="both", 28 | parameters=[{"robot_description": Command(["xacro ", xacro_file])}, {"use_sim_time": True}], 29 | ) 30 | 31 | bridge_config = os.path.join(get_package_share_directory("cart_pole_bringup"), "config", "gazebo_bridge.yaml") 32 | gz_bridge = Node( 33 | package="ros_gz_bridge", 34 | executable="parameter_bridge", 35 | name="ign_bridge", 36 | ros_arguments=["-p", f"config_file:={bridge_config}"], 37 | output="screen", 38 | ) 39 | 40 | simulation_control = Node( 41 | package="simulation_control", 42 | executable="simulation_control_node", 43 | parameters=[{"use_sim_time": True}], 44 | output="screen", 45 | ) 46 | 47 | simulation_observation = Node( 48 | package="cart_pole_observation", 49 | executable="cart_pole_observation_node", 50 | parameters=[{"use_sim_time": True}], 51 | output="screen", 52 | ) 53 | 54 | return LaunchDescription( 55 | [gazebo, gazebo_spawn_robot, robot_state_publisher, gz_bridge, simulation_control, simulation_observation] 56 | ) 57 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cart_pole_bringup 5 | 1.0.0 6 | The cart pole description and simulator launcher. 7 | Wiktor Bajor 8 | Apache 2.0 9 | 10 | Jakub Delicat 11 | 12 | ament_cmake 13 | 14 | ros_gz_bridge 15 | ros_gz_sim 16 | xacro 17 | robot_state_publisher 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/urdf/cart_pole.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 0 0 0.8 1 88 | 0 0 0.8 1 89 | 0 0 0.8 1 90 | 91 | 92 | 93 | 94 | 95 | 96 | 0.8 0 0 1 97 | 0.8 0 0 1 98 | 0.8 0 0 1 99 | 100 | 101 | 102 | 103 | 104 | slider_to_cart 105 | true 106 | effort_cmd 107 | 108 | 109 | joint_states 110 | slider_to_cart 111 | slider_to_pole_with_holder 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/urdf/cart_pole.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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 0 0 0.8 1 94 | 0 0 0.8 1 95 | 0 0 0.8 1 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 0.8 0 0 1 104 | 0.8 0 0 1 105 | 0.8 0 0 1 106 | 107 | 108 | 109 | 110 | 111 | 112 | slider_to_cart 113 | true 114 | effort_cmd 115 | 116 | 117 | joint_states 118 | slider_to_cart 119 | slider_to_pole_with_holder 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_bringup/urdf/macros.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/cart_pole_model_based_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/cart_pole_model_based_controller/cart_pole_model_based_controller/__init__.py -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/cart_pole_model_based_controller/cart_pole_system.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "## The cart pole problem\n", 8 | "![cart pole](../../../../img/cart_pole_system.png \" \")" 9 | ] 10 | }, 11 | { 12 | "cell_type": "markdown", 13 | "metadata": {}, 14 | "source": [ 15 | "## Below you can find the calculations to solve model control problem" 16 | ] 17 | }, 18 | { 19 | "cell_type": "code", 20 | "execution_count": null, 21 | "metadata": {}, 22 | "outputs": [], 23 | "source": [ 24 | "from sympy import Symbol, cos, sin, Rational, Matrix, Equality, simplify\n", 25 | "from sympy.physics.mechanics import dynamicsymbols, mechanics_printing\n", 26 | "from IPython.display import display\n", 27 | "\n", 28 | "mechanics_printing()\n", 29 | "\n", 30 | "time = Symbol(\"t\")\n", 31 | "cart_position = dynamicsymbols(\"x\")\n", 32 | "pole_angle = dynamicsymbols(\"theta\")\n", 33 | "cart_velocity = cart_position.diff()\n", 34 | "pole_angular_velocity = pole_angle.diff()\n", 35 | "pendulums_length = Symbol(\"l\")\n", 36 | "pendulum_mass_kg = Symbol(\"m\")\n", 37 | "pole_inertia = Symbol(\"i_p\")\n", 38 | "cart_mass_kg = Symbol(\"m_c\")\n", 39 | "g = Symbol(\"g\")\n", 40 | "\n", 41 | "x_pole_position = cart_position + pendulums_length * sin(pole_angle)\n", 42 | "y_pole_position = pendulums_length * cos(pole_angle)\n", 43 | "\n", 44 | "x_velocity_position = x_pole_position.diff(time)\n", 45 | "y_velocity_position = y_pole_position.diff(time)\n", 46 | "\n", 47 | "display(x_velocity_position)\n", 48 | "display(y_velocity_position)\n", 49 | "\n", 50 | "x_accelerations = x_velocity_position.diff(time)\n", 51 | "y_accelerations = y_velocity_position.diff(time)\n", 52 | "\n", 53 | "display(x_accelerations)\n", 54 | "display(y_accelerations)\n" 55 | ] 56 | }, 57 | { 58 | "cell_type": "markdown", 59 | "metadata": {}, 60 | "source": [ 61 | "## Velocities and accelerations of pendulum's end \n", 62 | "$$\n", 63 | "\\dot{x_p} = l \\cos{\\left(\\theta \\right)} \\dot{\\theta} + \\dot{x}\n", 64 | "$$\n", 65 | "\n", 66 | "$$\n", 67 | "\\dot{y_p} = - l \\sin{\\left(\\theta \\right)} \\dot{\\theta}\n", 68 | "$$\n", 69 | "\n", 70 | "$$\n", 71 | "\\ddot{x_p} = - l \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + l \\cos{\\left(\\theta \\right)} \\ddot{\\theta} + \\ddot{x}\n", 72 | "$$\n", 73 | "\n", 74 | "$$\n", 75 | "\\ddot{y_p} = - l \\sin{\\left(\\theta \\right)} \\ddot{\\\\theta} - l \\cos{\\left(\\theta \\right)} \\dot{\\theta}^{2}\n", 76 | "$$" 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "velocity = (x_velocity_position**2 + y_velocity_position**2).simplify()\n", 86 | "pole_kinetic_energy = (\n", 87 | " Rational(1, 2) * pendulum_mass_kg * velocity + Rational(1, 2) * pole_inertia * pole_angular_velocity**2\n", 88 | ")\n", 89 | "cart_kinetic_energy = Rational(1, 2) * cart_mass_kg * (cart_velocity**2)\n", 90 | "kinetic_energy = Matrix([pole_kinetic_energy, cart_kinetic_energy])\n", 91 | "\n", 92 | "display(kinetic_energy)\n", 93 | "\n", 94 | "potential_energy = Matrix([pendulum_mass_kg * g * pendulums_length * cos(pole_angle), 0])\n", 95 | "display(potential_energy)" 96 | ] 97 | }, 98 | { 99 | "cell_type": "markdown", 100 | "metadata": {}, 101 | "source": [ 102 | "## Pendulum's energies\n", 103 | "\n", 104 | "$$\n", 105 | "K = \\left[\\begin{matrix}\\frac{i_{p} \\dot{\\theta}^{2}}{2} + \\frac{m \\left(l^{2} \\dot{\\theta}^{2} + 2 l \\cos{\\left(\\theta \\right)} \\dot{\\theta} \\dot{x} + \\dot{x}^{2}\\right)}{2}\\\\\\frac{m_{c} \\dot{x}^{2}}{2}\\end{matrix}\\right]\n", 106 | "$$\n", 107 | "$$\n", 108 | "V = \\left[\\begin{matrix}g l m \\cos{\\left(\\theta \\right)}\\\\0\\end{matrix}\\right]\n", 109 | "$$" 110 | ] 111 | }, 112 | { 113 | "cell_type": "code", 114 | "execution_count": null, 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "lagrangian = kinetic_energy[0] + kinetic_energy[1] - potential_energy[0] - potential_energy[1]\n", 119 | "display(lagrangian)" 120 | ] 121 | }, 122 | { 123 | "cell_type": "markdown", 124 | "metadata": {}, 125 | "source": [ 126 | "## Lagrangian\n", 127 | "\n", 128 | "$$\n", 129 | "L = K - V\n", 130 | "$$\n", 131 | "\n", 132 | "$$\n", 133 | "L = - g l m \\cos{\\left(\\theta \\right)} + \\frac{i_{p} \\dot{\\theta}^{2}}{2} + \\frac{m \\left(l^{2} \\dot{\\theta}^{2} + 2 l \\cos{\\left(\\theta \\right)} \\dot{\\theta} \\dot{x} + \\dot{x}^{2}\\right)}{2} + \\frac{m_{c} \\dot{x}^{2}}{2}\n", 134 | "$$" 135 | ] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [ 143 | "q = Matrix([cart_position, pole_angle])\n", 144 | "qd = Matrix([cart_velocity, pole_angular_velocity])\n", 145 | "d_lagrangian_dx = lagrangian.diff(q)\n", 146 | "d_lagrangian_ddx = lagrangian.diff(qd)\n", 147 | "d_lagrangian_ddx_dt = d_lagrangian_ddx.diff(time)\n", 148 | "\n", 149 | "lagrangian_equation = d_lagrangian_ddx_dt - d_lagrangian_dx\n", 150 | "display(lagrangian_equation)" 151 | ] 152 | }, 153 | { 154 | "cell_type": "markdown", 155 | "metadata": {}, 156 | "source": [ 157 | "### Lagrange's equations\n", 158 | "\n", 159 | "$$\n", 160 | "L = \\left[\\begin{matrix}\\frac{m \\left(- 2 l \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + 2 l \\cos{\\left(\\theta \\right)} \\ddot{\\theta} + 2 \\ddot{x}\\right)}{2} + m_{c} \\ddot{x}\\\\- g l m \\sin{\\left(\\theta \\right)} + i_{p} \\ddot{\\theta} + l m \\sin{\\left(\\theta \\right)} \\dot{\\theta} \\dot{x} + \\frac{m \\left(2 l^{2} \\ddot{\\theta} - 2 l \\sin{\\left(\\theta \\right)} \\dot{\\theta} \\dot{x} + 2 l \\cos{\\left(\\theta \\right)} \\ddot{x}\\right)}{2}\\end{matrix}\\right]\n", 161 | "$$\n" 162 | ] 163 | }, 164 | { 165 | "cell_type": "code", 166 | "execution_count": null, 167 | "metadata": {}, 168 | "outputs": [], 169 | "source": [ 170 | "\n", 171 | "control_function = Symbol(\"f\")\n", 172 | "q2_lagrangian_equation = Equality(lagrangian_equation[1].simplify(), 0)\n", 173 | "display(q2_lagrangian_equation)\n", 174 | "angular_accelerations_coefficients = q2_lagrangian_equation.lhs.coeff(pole_angular_velocity.diff())\n", 175 | "q2_lagrangian_equation = Equality(q2_lagrangian_equation.lhs - angular_accelerations_coefficients * pole_angular_velocity.diff(), q2_lagrangian_equation.rhs - angular_accelerations_coefficients * pole_angular_velocity.diff()).simplify()\n", 176 | "display(q2_lagrangian_equation)\n", 177 | "q2_lagrangian_equation = Equality(q2_lagrangian_equation.lhs / angular_accelerations_coefficients, q2_lagrangian_equation.rhs / angular_accelerations_coefficients).simplify()\n", 178 | "display(q2_lagrangian_equation)\n", 179 | "\n", 180 | "q1_lagrangian_equation = Equality(lagrangian_equation[0].expand(), control_function)\n", 181 | "display(q1_lagrangian_equation)\n", 182 | "q1_lagrangian_equation = q1_lagrangian_equation.subs(pole_angular_velocity.diff(), q2_lagrangian_equation.rhs.simplify())\n", 183 | "display(q1_lagrangian_equation)\n", 184 | "q1_lagrangian_equation = Equality(q1_lagrangian_equation.lhs - control_function, q1_lagrangian_equation.rhs - control_function)\n", 185 | "display(q1_lagrangian_equation)\n", 186 | "q1_lagrangian_equation = Equality(q1_lagrangian_equation.lhs.expand(), q1_lagrangian_equation.rhs)\n", 187 | "display(q1_lagrangian_equation)\n", 188 | "cart_accelerations_coefficients = q1_lagrangian_equation.lhs.coeff(cart_velocity.diff())\n", 189 | "q1_lagrangian_equation = Equality(q1_lagrangian_equation.lhs - cart_accelerations_coefficients*cart_velocity.diff(), q1_lagrangian_equation.rhs - cart_accelerations_coefficients*cart_velocity.diff())\n", 190 | "display(q1_lagrangian_equation)\n", 191 | "q1_lagrangian_equation = Equality(q1_lagrangian_equation.lhs / cart_accelerations_coefficients, q1_lagrangian_equation.rhs / cart_accelerations_coefficients).simplify()\n", 192 | "display(q1_lagrangian_equation)\n", 193 | "\n", 194 | "q2_lagrangian_equation = q2_lagrangian_equation.subs(cart_velocity.diff(), q1_lagrangian_equation.rhs).simplify()\n", 195 | "display(q1_lagrangian_equation)\n" 196 | ] 197 | }, 198 | { 199 | "cell_type": "markdown", 200 | "metadata": {}, 201 | "source": [ 202 | "## Transforming Lagrange's equations\n", 203 | "\n", 204 | "Given following equations where **f** is cart momentum:\n", 205 | "\n", 206 | "$$\n", 207 | "- l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + l m \\cos{\\left(\\theta \\right)} \\ddot{\\theta} + m \\ddot{x} + m_{c} \\ddot{x} = f\n", 208 | "$$\n", 209 | "\n", 210 | "$$\n", 211 | "- g l m \\sin{\\left(\\theta \\right)} + i_{p} \\ddot{\\theta} + l^{2} m \\ddot{\\theta} + l m \\cos{\\left(\\theta \\right)} \\ddot{x} = 0\n", 212 | "$$\n", 213 | "\n", 214 | "\n", 215 | "Using given equations the $\\dot{X}$ will be derived:\n", 216 | "$$\n", 217 | "\\dot{X} = \n", 218 | "\\left[\\begin{matrix}\\dot{x}\\\\\\dot{\\theta}\\\\\\ddot{x}\\\\\\ddot{\\theta}\\end{matrix}\\right]\n", 219 | "$$\n", 220 | "\n", 221 | "\n", 222 | "Transforming second equation:\n", 223 | "\n", 224 | "$$\n", 225 | "- g l m \\sin{\\left(\\theta \\right)} + i_{p} \\ddot{\\theta} + l^{2} m \\ddot{\\theta} + l m \\cos{\\left(\\theta \\right)} \\ddot{x} = 0\n", 226 | "$$\n", 227 | "\n", 228 | "$$\n", 229 | "\\left(i_{p} + l^{2} m\\right) \\ddot{\\theta} = - l m \\left(- g \\sin{\\left(\\theta \\right)} + \\cos{\\left(\\theta \\right)} \\ddot{x}\\right)\n", 230 | "$$\n", 231 | "\n", 232 | "$$\n", 233 | "\\ddot{\\theta} = \\frac{l m \\left(g \\sin{\\left(\\theta \\right)} - \\cos{\\left(\\theta \\right)} \\ddot{x}\\right)}{i_{p} + l^{2} m}\n", 234 | "$$\n", 235 | "\n", 236 | "\n", 237 | "Transforming first equation and substitute second equation as $\\ddot{\\theta}$\n", 238 | "\n", 239 | "$$\n", 240 | "\\frac{l^{2} m^{2} \\left(g \\sin{\\left(\\theta \\right)} - \\cos{\\left(\\theta \\right)} \\ddot{x}\\right) \\cos{\\left(\\theta \\right)}}{i_{p} + l^{2} m} - l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + m \\ddot{x} + m_{c} \\ddot{x} = f\n", 241 | "$$\n", 242 | "\n", 243 | "$$\n", 244 | "- f + \\frac{l^{2} m^{2} \\left(g \\sin{\\left(\\theta \\right)} - \\cos{\\left(\\theta \\right)} \\ddot{x}\\right) \\cos{\\left(\\theta \\right)}}{i_{p} + l^{2} m} - l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + m \\ddot{x} + m_{c} \\ddot{x} = 0\n", 245 | "$$\n", 246 | "\n", 247 | "$$\n", 248 | "- f + \\frac{g l^{2} m^{2} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)}}{i_{p} + l^{2} m} - \\frac{l^{2} m^{2} \\cos^{2}{\\left(\\theta \\right)} \\ddot{x}}{i_{p} + l^{2} m} - l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + m \\ddot{x} + m_{c} \\ddot{x} = 0\n", 249 | "$$\n", 250 | "$$\n", 251 | "- f + \\frac{g l^{2} m^{2} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)}}{i_{p} + l^{2} m} - \\frac{l^{2} m^{2} \\cos^{2}{\\left(\\theta \\right)} \\ddot{x}}{i_{p} + l^{2} m} - l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + m \\ddot{x} + m_{c} \\ddot{x} - \\left(- \\frac{l^{2} m^{2} \\cos^{2}{\\left(\\theta \\right)}}{i_{p} + l^{2} m} + m + m_{c}\\right) \\ddot{x} = - \\left(- \\frac{l^{2} m^{2} \\cos^{2}{\\left(\\theta \\right)}}{i_{p} + l^{2} m} + m + m_{c}\\right) \\ddot{x}\n", 252 | "$$\n", 253 | "\n", 254 | "After substituting the $\\ddot{x}$ to the second equation we get:\n", 255 | "\n", 256 | "$$\n", 257 | "\\ddot{x} = - \\frac{- f i_{p} - f l^{2} m + \\frac{g l^{2} m^{2} \\sin{\\left(2 \\theta \\right)}}{2} - i_{p} l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} - l^{3} m^{2} \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2}}{i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}}\n", 258 | "$$\n", 259 | "\n", 260 | "$$\n", 261 | "\\ddot{\\theta} = \\frac{l m \\left(2 g \\left(i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}\\right) \\sin{\\left(\\theta \\right)} - \\left(2 f i_{p} + 2 f l^{2} m - g l^{2} m^{2} \\sin{\\left(2 \\theta \\right)} + 2 i_{p} l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + 2 l^{3} m^{2} \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2}\\right) \\cos{\\left(\\theta \\right)}\\right)}{2 \\left(i_{p} + l^{2} m\\right) \\left(i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}\\right)}\n", 262 | "$$\n", 263 | "\n", 264 | "\n", 265 | "We can rewrite it as:\n", 266 | "\n", 267 | "$$\n", 268 | "\n", 269 | "\\dot{X} = \n", 270 | "\\left[\\begin{matrix}\\dot{x}\\\\\\dot{\\theta}\\\\\\ddot{x}\\\\\\ddot{\\theta}\\end{matrix}\\right]\n", 271 | "= \\left[\\begin{matrix}\\dot{x}\\\\\\dot{\\theta}\\\\ - \\frac{- f i_{p} - f l^{2} m + \\frac{g l^{2} m^{2} \\sin{\\left(2 \\theta \\right)}}{2} - i_{p} l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} - l^{3} m^{2} \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2}}{i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}} \\\\ \n", 272 | "\\frac{l m \\left(2 g \\left(i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}\\right) \\sin{\\left(\\theta \\right)} - \\left(2 f i_{p} + 2 f l^{2} m - g l^{2} m^{2} \\sin{\\left(2 \\theta \\right)} + 2 i_{p} l m \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2} + 2 l^{3} m^{2} \\sin{\\left(\\theta \\right)} \\dot{\\theta}^{2}\\right) \\cos{\\left(\\theta \\right)}\\right)}{2 \\left(i_{p} + l^{2} m\\right) \\left(i_{p} m + i_{p} m_{c} + l^{2} m^{2} \\sin^{2}{\\left(\\theta \\right)} + l^{2} m m_{c}\\right)} \\end{matrix}\\right]\n", 273 | "\n", 274 | "$$\n" 275 | ] 276 | }, 277 | { 278 | "cell_type": "markdown", 279 | "metadata": {}, 280 | "source": [ 281 | "## Linearization in the equilibrium point\n", 282 | "\n", 283 | "### Calculate the jacobian in respect to state matrix and input function to obtain state matrix and input vector\n", 284 | "\n", 285 | "### Use 0 as value for the pole_angle/$\\theta$ \n" 286 | ] 287 | }, 288 | { 289 | "cell_type": "code", 290 | "execution_count": null, 291 | "metadata": {}, 292 | "outputs": [], 293 | "source": [ 294 | "state_derivative = Matrix([cart_velocity, pole_angular_velocity, q1_lagrangian_equation.rhs.expand(), q2_lagrangian_equation.rhs.simplify()])\n", 295 | "state_matrix = state_derivative.jacobian([cart_position, pole_angle, cart_velocity, pole_angular_velocity]).subs(pole_angle, 0)\n", 296 | "input_matrix = state_derivative.jacobian([control_function]).subs(pole_angle, 0)\n", 297 | "\n", 298 | "display(simplify(Matrix(state_matrix)))\n", 299 | "display(simplify(Matrix(input_matrix)))" 300 | ] 301 | }, 302 | { 303 | "cell_type": "markdown", 304 | "metadata": {}, 305 | "source": [ 306 | "## In result we get\n", 307 | "\n", 308 | "\n", 309 | "$$\n", 310 | "\n", 311 | "\\left[\\begin{matrix}\\dot{x}\\\\\\dot{\\theta}\\\\\\ddot{x}\\\\\\ddot{\\theta}\\end{matrix}\\right]\n", 312 | "= \n", 313 | "\\left[\\begin{matrix}0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\\\0 & - \\frac{g l^{2} m^{2}}{i_{p} m + i_{p} m_{c} + l^{2} m m_{c}} & 0 & 0\\\\0 & \\frac{l m \\left(g m + g m_{c}\\right)}{i_{p} m + i_{p} m_{c} + l^{2} m m_{c}} & 0 & 0\\end{matrix}\\right] \\left[\\begin{matrix}x\\\\\\theta\\\\\\dot{x}\\\\\\dot{\\theta}\\end{matrix}\\right] + \\left[\\begin{matrix}0\\\\0\\\\\\frac{i_{p} + l^{2} m}{i_{p} m + i_{p} m_{c} + l^{2} m m_{c}}\\\\- \\frac{l m}{i_{p} m + i_{p} m_{c} + l^{2} m m_{c}}\\end{matrix}\\right] f\n", 314 | "\n", 315 | "$$\n" 316 | ] 317 | }, 318 | { 319 | "cell_type": "markdown", 320 | "metadata": {}, 321 | "source": [ 322 | "## Controller creation \n", 323 | "\n", 324 | "Using pole placement method to create controller." 325 | ] 326 | }, 327 | { 328 | "cell_type": "code", 329 | "execution_count": 2, 330 | "metadata": {}, 331 | "outputs": [], 332 | "source": [ 333 | "import numpy as np\n", 334 | "import control\n", 335 | "from scipy.signal import lsim, lti\n", 336 | "import matplotlib.pyplot as plt\n", 337 | "\n", 338 | "def get_state_matrix(length_to_pendulums_center_mass, pendulum_mass_kg, cart_mass_kg, pole_inertia):\n", 339 | " g = 9.81\n", 340 | " return np.array(\n", 341 | " [\n", 342 | " [0, 0, 1, 0],\n", 343 | " [0, 0, 0, 1],\n", 344 | " [\n", 345 | " 0,\n", 346 | " -g\n", 347 | " * length_to_pendulums_center_mass**2\n", 348 | " * pendulum_mass_kg**2\n", 349 | " / (\n", 350 | " pole_inertia * cart_mass_kg\n", 351 | " + pole_inertia * pendulum_mass_kg\n", 352 | " + cart_mass_kg * length_to_pendulums_center_mass**2 * pendulum_mass_kg\n", 353 | " ),\n", 354 | " 0,\n", 355 | " 0,\n", 356 | " ],\n", 357 | " [\n", 358 | " 0,\n", 359 | " g\n", 360 | " * length_to_pendulums_center_mass\n", 361 | " * pendulum_mass_kg\n", 362 | " * (cart_mass_kg + pendulum_mass_kg)\n", 363 | " / (\n", 364 | " pole_inertia * cart_mass_kg\n", 365 | " + pole_inertia * pendulum_mass_kg\n", 366 | " + cart_mass_kg * length_to_pendulums_center_mass**2 * pendulum_mass_kg\n", 367 | " ),\n", 368 | " 0,\n", 369 | " 0,\n", 370 | " ],\n", 371 | " ]\n", 372 | " )\n", 373 | "\n", 374 | "\n", 375 | "def get_input_matrix(length_to_pendulums_center_mass, pendulum_mass_kg, cart_mass_kg, pole_inertia):\n", 376 | " return np.array(\n", 377 | " [\n", 378 | " [0],\n", 379 | " [0],\n", 380 | " [\n", 381 | " (-pole_inertia - length_to_pendulums_center_mass**2 * pendulum_mass_kg)\n", 382 | " / (\n", 383 | " -pole_inertia * cart_mass_kg\n", 384 | " - pole_inertia * pendulum_mass_kg\n", 385 | " - cart_mass_kg * length_to_pendulums_center_mass**2 * pendulum_mass_kg\n", 386 | " )\n", 387 | " ],\n", 388 | " [\n", 389 | " length_to_pendulums_center_mass\n", 390 | " * pendulum_mass_kg\n", 391 | " / (\n", 392 | " -pole_inertia * cart_mass_kg\n", 393 | " - pole_inertia * pendulum_mass_kg\n", 394 | " - cart_mass_kg * length_to_pendulums_center_mass**2 * pendulum_mass_kg\n", 395 | " )\n", 396 | " ],\n", 397 | " ]\n", 398 | " )\n" 399 | ] 400 | }, 401 | { 402 | "cell_type": "code", 403 | "execution_count": 3, 404 | "metadata": {}, 405 | "outputs": [ 406 | { 407 | "name": "stdout", 408 | "output_type": "stream", 409 | "text": [ 410 | "Controllability matrix's rank: 4\n", 411 | "Eigen values of state matrix (A): [ 0. 0. 4.33373483 -4.33373483]\n" 412 | ] 413 | } 414 | ], 415 | "source": [ 416 | "pendulum_mass_kg = 0.455\n", 417 | "cart_mass_kg = 1.006\n", 418 | "length_to_pendulums_center_mass = 0.435057\n", 419 | "pole_inertia = 0.044096\n", 420 | "\n", 421 | "state_matrix = get_state_matrix(length_to_pendulums_center_mass, pendulum_mass_kg, cart_mass_kg, pole_inertia)\n", 422 | "input_matrix = get_input_matrix(length_to_pendulums_center_mass, pendulum_mass_kg, cart_mass_kg, pole_inertia)\n", 423 | "controllability_matrix = control.ctrb(state_matrix, input_matrix)\n", 424 | "\n", 425 | "\n", 426 | "print(f\"Controllability matrix's rank: {np.linalg.matrix_rank(controllability_matrix, tol=1.0e-10)}\")\n", 427 | "print(f\"Eigen values of state matrix (A): {np.linalg.eigvals(state_matrix)}\")" 428 | ] 429 | }, 430 | { 431 | "cell_type": "code", 432 | "execution_count": 4, 433 | "metadata": {}, 434 | "outputs": [ 435 | { 436 | "name": "stdout", 437 | "output_type": "stream", 438 | "text": [ 439 | "Controller (K): [[ -1.86696764 -42.26984303 -3.88951591 -10.18982873]]\n", 440 | "System with new eigen values after adding controller: [-4. -3. -2. -1.]\n", 441 | "Controllability matrix's rank: 4\n" 442 | ] 443 | } 444 | ], 445 | "source": [ 446 | "poles = [-1, -2, -3, -4]\n", 447 | "controller = control.place(state_matrix, input_matrix, poles)\n", 448 | "print(f\"Controller (K): {controller}\")\n", 449 | "\n", 450 | "state_matrix_new_eigen_values = state_matrix - input_matrix @ controller\n", 451 | "print(f\"System with new eigen values after adding controller: {np.linalg.eigvals(state_matrix_new_eigen_values)}\")\n", 452 | "\n", 453 | "controllability_matrix_with_new_eigen_values = control.ctrb(state_matrix_new_eigen_values, input_matrix)\n", 454 | "print(\n", 455 | " f\"Controllability matrix's rank: {np.linalg.matrix_rank(controllability_matrix_with_new_eigen_values, tol=1.0e-10)}\"\n", 456 | ")" 457 | ] 458 | }, 459 | { 460 | "cell_type": "code", 461 | "execution_count": 5, 462 | "metadata": {}, 463 | "outputs": [ 464 | { 465 | "name": "stdout", 466 | "output_type": "stream", 467 | "text": [ 468 | "State: [0.5 0.1 0. 0.1]\n", 469 | "States to use in controller: [0 1 0 1]\n", 470 | "Controller with certain states being used: [[ -0. -42.26984303 -0. -10.18982873]]\n", 471 | "Controller's: [-5.24596718]\n" 472 | ] 473 | } 474 | ], 475 | "source": [ 476 | "states_used_in_control_loop = np.array([0, 1, 0, 1])\n", 477 | "state_vector = np.array([0.5, 0.1, 0, 0.1])\n", 478 | "\n", 479 | "print(f\"State: {state_vector}\")\n", 480 | "print(f\"States to use in controller: {states_used_in_control_loop}\")\n", 481 | "print(f\"Controller with certain states being used: {controller * states_used_in_control_loop}\")\n", 482 | "print(f\"Controller's: {np.dot(controller * states_used_in_control_loop, state_vector)}\")" 483 | ] 484 | }, 485 | { 486 | "cell_type": "code", 487 | "execution_count": 6, 488 | "metadata": {}, 489 | "outputs": [ 490 | { 491 | "data": { 492 | "image/png": "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", 493 | "text/plain": [ 494 | "
" 495 | ] 496 | }, 497 | "metadata": {}, 498 | "output_type": "display_data" 499 | } 500 | ], 501 | "source": [ 502 | "D = 0.0\n", 503 | "system = lti(state_matrix_new_eigen_values, input_matrix, states_used_in_control_loop, D)\n", 504 | "time = np.linspace(0, 50, num=200)\n", 505 | "u = np.ones_like(time)\n", 506 | "_, y, x = lsim(system, u, time)\n", 507 | "plt.plot(time, y)\n", 508 | "plt.grid(alpha=0.3)\n", 509 | "plt.xlabel(\"time\")\n", 510 | "plt.show()" 511 | ] 512 | } 513 | ], 514 | "metadata": { 515 | "kernelspec": { 516 | "display_name": "Python 3", 517 | "language": "python", 518 | "name": "python3" 519 | }, 520 | "language_info": { 521 | "codemirror_mode": { 522 | "name": "ipython", 523 | "version": 3 524 | }, 525 | "file_extension": ".py", 526 | "mimetype": "text/x-python", 527 | "name": "python", 528 | "nbconvert_exporter": "python", 529 | "pygments_lexer": "ipython3", 530 | "version": "3.10.12" 531 | } 532 | }, 533 | "nbformat": 4, 534 | "nbformat_minor": 2 535 | } 536 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/cart_pole_model_based_controller/linear_approximation_control_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from cart_pole_reinforcement_learning.cart_pole_learning_control_node import CartPoleReinforcementLearning 3 | from std_msgs.msg import Float64 4 | import numpy as np 5 | import control 6 | 7 | 8 | class LinearApproximationControlNode(CartPoleReinforcementLearning): 9 | def __init__(self, node_name="linear_approximation_control_node"): 10 | super().__init__(node_name) 11 | self.pendulum_mass_kg = 0.455 12 | self.cart_mass_kg = 1.006 13 | self.length_to_pendulums_center_mass = 0.435057 14 | self.pole_inertia = 0.044096 15 | 16 | state_matrix = self.get_state_matrix() 17 | input_matrix = self.get_input_matrix() 18 | self.controller = control.place(state_matrix, input_matrix, [-3, -1, -4, -2]) 19 | self.states_used_in_control_loop = np.array([1, 1, 1, 1]) 20 | self.timer = self.create_timer(timer_period_sec=0.1, callback=self.run) 21 | 22 | def get_state_matrix(self) -> np.array: 23 | g = 9.81 24 | return np.array( 25 | [ 26 | [0, 0, 1, 0], 27 | [0, 0, 0, 1], 28 | [ 29 | 0, 30 | -g 31 | * self.length_to_pendulums_center_mass**2 32 | * self.pendulum_mass_kg**2 33 | / ( 34 | self.pole_inertia * self.cart_mass_kg 35 | + self.pole_inertia * self.pendulum_mass_kg 36 | + self.cart_mass_kg * self.length_to_pendulums_center_mass**2 * self.pendulum_mass_kg 37 | ), 38 | 0, 39 | 0, 40 | ], 41 | [ 42 | 0, 43 | g 44 | * self.length_to_pendulums_center_mass 45 | * self.pendulum_mass_kg 46 | * (self.cart_mass_kg + self.pendulum_mass_kg) 47 | / ( 48 | self.pole_inertia * self.cart_mass_kg 49 | + self.pole_inertia * self.pendulum_mass_kg 50 | + self.cart_mass_kg * self.length_to_pendulums_center_mass**2 * self.pendulum_mass_kg 51 | ), 52 | 0, 53 | 0, 54 | ], 55 | ] 56 | ) 57 | 58 | def get_input_matrix(self) -> np.array: 59 | return np.array( 60 | [ 61 | [0], 62 | [0], 63 | [ 64 | (-self.pole_inertia - self.length_to_pendulums_center_mass**2 * self.pendulum_mass_kg) 65 | / ( 66 | -self.pole_inertia * self.cart_mass_kg 67 | - self.pole_inertia * self.pendulum_mass_kg 68 | - self.cart_mass_kg * self.length_to_pendulums_center_mass**2 * self.pendulum_mass_kg 69 | ) 70 | ], 71 | [ 72 | self.length_to_pendulums_center_mass 73 | * self.pendulum_mass_kg 74 | / ( 75 | -self.pole_inertia * self.cart_mass_kg 76 | - self.pole_inertia * self.pendulum_mass_kg 77 | - self.cart_mass_kg * self.length_to_pendulums_center_mass**2 * self.pendulum_mass_kg 78 | ) 79 | ], 80 | ] 81 | ) 82 | 83 | def get_control_value(self) -> float: 84 | return -np.dot(self.controller * self.states_used_in_control_loop, self.get_cart_observations())[0] 85 | 86 | def run(self): 87 | if not self.is_simulation_ready(): 88 | return 89 | 90 | if self.is_simulation_stopped(): 91 | self.restart_learning_loop() 92 | self.get_logger().info("Simulation stopped. Restarting...") 93 | return 94 | 95 | self.take_action(Float64(data=self.get_control_value())) 96 | 97 | 98 | def main(args=None): 99 | rclpy.init(args=args) 100 | rclpy.spin(LinearApproximationControlNode()) 101 | rclpy.shutdown() 102 | 103 | 104 | if __name__ == "__main__": 105 | main() 106 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cart_pole_model_based_controller 5 | 1.0.0 6 | Cart pole dynamic model based controller 7 | Jakub Delicat 8 | Wiktor Bajor 9 | 10 | Apache-2.0 11 | 12 | std_msgs 13 | cart_pole_observation_interface 14 | std_srvs 15 | python-control-pip 16 | python-scipy 17 | python-sympy 18 | jupyter-nbconvert 19 | ipython3 20 | 21 | 22 | ament_python 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/resource/cart_pole_model_based_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/cart_pole_model_based_controller/resource/cart_pole_model_based_controller -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/cart_pole_model_based_controller 3 | [install] 4 | install_scripts=$base/lib/cart_pole_model_based_controller 5 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_model_based_controller/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "cart_pole_model_based_controller" 4 | 5 | setup( 6 | name=package_name, 7 | version="0.0.0", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="Jakub Delicat", 16 | maintainer_email="delicat.kuba@gmail.com", 17 | description="Cart pole dynamic model based controller", 18 | license="Apache-2.0", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": [ 22 | "linear_approximation_control_node = cart_pole_model_based_controller.linear_approximation_control_node:main", 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cart_pole_observation) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(sensor_msgs REQUIRED) 11 | find_package(cart_pole_observation_interface REQUIRED) 12 | 13 | add_executable(cart_pole_observation_node src/cart_pole_observation_node.cpp) 14 | target_compile_features(cart_pole_observation_node PUBLIC c_std_99 cxx_std_17) 15 | ament_target_dependencies( 16 | cart_pole_observation_node 17 | "rclcpp" 18 | "sensor_msgs" 19 | "cart_pole_observation_interface" 20 | ) 21 | 22 | install(TARGETS cart_pole_observation_node DESTINATION lib/${PROJECT_NAME}) 23 | 24 | ament_package() 25 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cart_pole_observation 5 | 1.0.0 6 | Simulation observation publisher 7 | Wiktor Bajor 8 | Apache 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | cart_pole_observation_interface 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation/src/cart_pole_observation_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "cart_pole_observation_interface/msg/cart_pole_observation.hpp" 6 | 7 | using cart_pole_observation_interface::msg::CartPoleObservation; 8 | 9 | class CartPoleObserver : public rclcpp::Node 10 | { 11 | public: 12 | CartPoleObserver() : Node("cart_pole_observer") 13 | { 14 | cart_pole_state_publisher_ = create_publisher("observations", 10); 15 | auto topic_callback = [this](sensor_msgs::msg::JointState::UniquePtr msg) 16 | { 17 | const auto slider_to_cart_iter = std::find(msg->name.cbegin(), msg->name.cend(), "slider_to_cart"); 18 | const auto pole_holder_base_to_pole_holder_iter = 19 | std::find(msg->name.cbegin(), msg->name.cend(), "slider_to_pole_with_holder"); 20 | CartPoleObservation cart_pole_observation; 21 | cart_pole_observation.cart_position = msg->position[std::distance(msg->name.cbegin(), slider_to_cart_iter)]; 22 | cart_pole_observation.cart_velocity = msg->velocity[std::distance(msg->name.cbegin(), slider_to_cart_iter)]; 23 | cart_pole_observation.pole_angle = 24 | msg->position[std::distance(msg->name.cbegin(), pole_holder_base_to_pole_holder_iter)]; 25 | cart_pole_observation.pole_angular_velocity = 26 | msg->velocity[std::distance(msg->name.cbegin(), pole_holder_base_to_pole_holder_iter)]; 27 | cart_pole_state_publisher_->publish(cart_pole_observation); 28 | }; 29 | joint_states_subscriber_ = create_subscription("joint_states", 10, topic_callback); 30 | } 31 | 32 | private: 33 | rclcpp::Publisher::SharedPtr cart_pole_state_publisher_; 34 | rclcpp::Subscription::SharedPtr joint_states_subscriber_; 35 | }; 36 | 37 | int main(int argc, char* argv[]) 38 | { 39 | rclcpp::init(argc, argv); 40 | rclcpp::spin(std::make_shared()); 41 | rclcpp::shutdown(); 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cart_pole_observation_interface) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rosidl_default_generators REQUIRED) 10 | 11 | rosidl_generate_interfaces(${PROJECT_NAME} 12 | "msg/CartPoleObservation.msg" 13 | ) 14 | 15 | ament_package() 16 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation_interface/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation_interface/msg/CartPoleObservation.msg: -------------------------------------------------------------------------------- 1 | float64 cart_position 2 | float64 cart_velocity 3 | float64 pole_angle 4 | float64 pole_angular_velocity 5 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_observation_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cart_pole_observation_interface 5 | 1.0.0 6 | Cart pole observation interface. 7 | Wiktor Bajor 8 | Apache 2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | rosidl_default_runtime 14 | rosidl_interface_packages 15 | 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/__init__.py -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/cart_pole_basic_policy_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from cart_pole_reinforcement_learning.reinforcement_learning_node import ReinforcementLearningNode 3 | 4 | 5 | class CartPoleReinforcementBasicPolicy(ReinforcementLearningNode): 6 | def __init__(self): 7 | super().__init__("cart_pole_basic_policy_node") 8 | self.create_timer(0.05, self.run) 9 | 10 | def run_one_step(self): 11 | self.take_action(self.create_command(int(self.get_cart_observations()[1] < 0))) 12 | self.step += 1 13 | 14 | def run(self): 15 | if not self.is_simulation_ready(): 16 | return 17 | 18 | self.stop_run_when_learning_ended() 19 | self.advance_episode_when_finished() 20 | self.run_one_step() 21 | 22 | 23 | def main(args=None): 24 | rclpy.init(args=args) 25 | rclpy.spin(CartPoleReinforcementBasicPolicy()) 26 | rclpy.shutdown() 27 | 28 | 29 | if __name__ == "__main__": 30 | main() 31 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/cart_pole_deep_q_learning_policy_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from cart_pole_reinforcement_learning.reinforcement_learning_node import ReinforcementLearningNode 3 | import tensorflow as tf 4 | import numpy as np 5 | from collections import deque 6 | 7 | 8 | class CartPoleReinforcementDeepQLearningPolicy(ReinforcementLearningNode): 9 | def __init__(self): 10 | self.BATCH_SIZE = 32 11 | self.NUMBER_OF_OUTPUTS = 2 12 | self.NUMBER_OF_EXPERIMENT_RESULTS = 6 13 | self.NUMBER_OF_EPISODES_BEFORE_LEARNING = 50 14 | super().__init__( 15 | name="cart_pole_deep_q_learning_policy_node", 16 | optimizer=tf.keras.optimizers.Nadam(learning_rate=1e-3), 17 | loss_function=tf.keras.losses.mse, 18 | model=self.create_model(), 19 | ) 20 | self.state = self.get_cart_observations() 21 | self.replay_buffer = deque(maxlen=2000) 22 | self.create_timer(0.05, self.run) 23 | 24 | def create_model(self): 25 | return tf.keras.Sequential( 26 | [ 27 | tf.keras.layers.Input(shape=(4,)), 28 | tf.keras.layers.Dense(32, activation="elu"), 29 | tf.keras.layers.Dense(32, activation="elu"), 30 | tf.keras.layers.Dense(32, activation="elu"), 31 | tf.keras.layers.Dense(32, activation="elu"), 32 | tf.keras.layers.Dense(self.NUMBER_OF_OUTPUTS), 33 | ] 34 | ) 35 | 36 | def epsilon_greedy_policy(self, epsilon, state): 37 | if np.random.rand() < epsilon: 38 | return np.random.randint(self.NUMBER_OF_OUTPUTS) 39 | 40 | return self.model.predict(state[np.newaxis], verbose=0)[0].argmax() 41 | 42 | def sample_experiences(self): 43 | indices = np.random.randint(len(self.replay_buffer), size=self.BATCH_SIZE) 44 | batch = [self.replay_buffer[index] for index in indices] 45 | return [ 46 | np.array([experience[field_index] for experience in batch]) 47 | for field_index in range(self.NUMBER_OF_EXPERIMENT_RESULTS) 48 | ] 49 | 50 | def run_one_step(self, epsilon): 51 | self.action = self.epsilon_greedy_policy(epsilon, np.array(self.get_cart_observations())) 52 | self.take_action(self.create_command(self.action)) 53 | self.step += 1 54 | 55 | def training_step(self): 56 | states, actions, rewards, next_states, dones, truncateds = self.sample_experiences() 57 | target_q_values = self.calculate_target_q_values(rewards, next_states, dones, truncateds) 58 | grads = self.calculate_gradient(states, actions, target_q_values) 59 | self.optimizer.apply_gradients(zip(grads, self.model.trainable_variables)) 60 | 61 | def calculate_gradient(self, states, actions, target_q_values): 62 | mask = tf.one_hot(actions, self.NUMBER_OF_OUTPUTS) 63 | with tf.GradientTape() as tape: 64 | all_q_values = self.model(states) 65 | q_values = tf.reduce_sum(all_q_values * mask, axis=1, keepdims=True) 66 | loss = tf.reduce_mean(self.loss_function(target_q_values, q_values)) 67 | 68 | grads = tape.gradient(loss, self.model.trainable_variables) 69 | return grads 70 | 71 | def calculate_target_q_values(self, rewards, next_states, dones, truncateds): 72 | max_next_q_values = self.model.predict(next_states, verbose=0).max(axis=1) 73 | runs = 1.0 - (dones | truncateds) 74 | target_q_values = rewards + runs * self.discount_factor * max_next_q_values 75 | target_q_values = target_q_values.reshape(-1, 1) 76 | return target_q_values 77 | 78 | def learn_neural_network(self): 79 | if self.episode > self.NUMBER_OF_EPISODES_BEFORE_LEARNING and ( 80 | self.is_episode_ended() or self.is_simulation_stopped() 81 | ): 82 | self.training_step() 83 | 84 | def run(self): 85 | if not self.is_simulation_ready(): 86 | return 87 | self.learn_neural_network() 88 | self.stop_run_when_learning_ended() 89 | self.advance_episode_when_finished() 90 | self.run_one_step(epsilon=max(1 - self.episode / self.max_number_of_episodes, 0.01)) 91 | self.append_data_from_last_step() 92 | 93 | def append_data_from_last_step(self): 94 | self.replay_buffer.append( 95 | ( 96 | self.state, 97 | self.action, 98 | self.reward, 99 | self.get_cart_observations(), 100 | self.is_episode_ended(), 101 | self.is_simulation_stopped(), 102 | ) 103 | ) 104 | self.state = self.get_cart_observations() 105 | 106 | 107 | def main(args=None): 108 | rclpy.init(args=args) 109 | rclpy.spin(CartPoleReinforcementDeepQLearningPolicy()) 110 | rclpy.shutdown() 111 | 112 | 113 | if __name__ == "__main__": 114 | main() 115 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/cart_pole_learning_control_node.py: -------------------------------------------------------------------------------- 1 | from rclpy.node import Node 2 | from cart_pole_observation_interface.msg import CartPoleObservation 3 | from std_msgs.msg import Float64 4 | import numpy as np 5 | from std_srvs.srv import Empty 6 | from rclpy.task import Future 7 | from rclpy.publisher import Publisher 8 | from rclpy.subscription import Subscription 9 | from rclpy.client import Client 10 | 11 | 12 | class CartPoleReinforcementLearning(Node): 13 | def __init__(self, node_name="cart_pole_learning_node"): 14 | super().__init__(node_name) 15 | self.observation_subscriber: Subscription = self.create_subscription( 16 | CartPoleObservation, "observations", self.store_observation, 10 17 | ) 18 | self.simulation_reset_service_client: Client = self.create_client(Empty, "restart_sim_service") 19 | self.effort_command_publisher: Publisher = self.create_publisher(Float64, "effort_cmd", 10) 20 | self.cart_observations: list[float] = [0.0, 0.0, 0.0, 0.0] 21 | self.is_truncated: bool = False 22 | self.UPPER_POLE_LIMIT: float = 1.4 23 | self.LOWER_POLE_LIMIT: float = -1.4 24 | self.restarting_future: Future = None 25 | 26 | def store_observation(self, cart_pole_observation: CartPoleObservation): 27 | self.cart_observations[0] = cart_pole_observation.cart_position 28 | self.cart_observations[1] = cart_pole_observation.pole_angle 29 | self.cart_observations[2] = cart_pole_observation.cart_velocity 30 | self.cart_observations[3] = cart_pole_observation.pole_angular_velocity 31 | self.update_simulation_status() 32 | 33 | def get_cart_observations(self) -> list[float]: 34 | return [obs for obs in self.cart_observations] 35 | 36 | def is_simulation_stopped(self) -> bool: 37 | return self.is_truncated 38 | 39 | def take_action(self, action: Float64): 40 | self.effort_command_publisher.publish(action) 41 | 42 | def reset_observation(self): 43 | self.cart_observations = [0.0, 0.0, 0.0, 0.0] 44 | self.is_truncated = False 45 | 46 | def update_simulation_status(self): 47 | self.is_truncated = np.isclose( 48 | self.cart_observations[1], 49 | self.UPPER_POLE_LIMIT, 50 | rtol=1e-05, 51 | atol=1e-08, 52 | equal_nan=False, 53 | ) or np.isclose( 54 | self.cart_observations[1], 55 | self.LOWER_POLE_LIMIT, 56 | rtol=1e-05, 57 | atol=1e-08, 58 | equal_nan=False, 59 | ) 60 | 61 | def restart_simulation(self): 62 | while not self.simulation_reset_service_client.wait_for_service(timeout_sec=1.0): 63 | self.get_logger().info("restart_sim_service not available, waiting again...") 64 | self.restarting_future = self.simulation_reset_service_client.call_async(Empty.Request()) 65 | 66 | def is_simulation_ready(self) -> bool: 67 | if self.restarting_future is None: 68 | return True 69 | try: 70 | return self.restarting_future.done() 71 | except: 72 | return False 73 | 74 | def restart_learning_loop(self): 75 | self.restart_simulation() 76 | self.reset_observation() 77 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/cart_pole_neural_network_policy.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from cart_pole_reinforcement_learning.reinforcement_learning_node import ReinforcementLearningNode 3 | import tensorflow as tf 4 | import copy 5 | import numpy as np 6 | 7 | 8 | class CartPoleReinforcementNeuralNetworkPolicy(ReinforcementLearningNode): 9 | def __init__(self): 10 | super().__init__( 11 | name="cart_pole_neural_network_node", 12 | optimizer=tf.keras.optimizers.Nadam(learning_rate=1e-3), 13 | loss_function=tf.keras.losses.binary_crossentropy, 14 | model=self.create_model(), 15 | ) 16 | 17 | self.iteration = 0 18 | self.max_number_of_iterations = 100 19 | self.max_number_of_episodes_per_iteration = 20 20 | self.rewards = [] 21 | self.gradients = [] 22 | self.episode_rewards = [] 23 | self.episode_gradients = [] 24 | self.create_timer(0.05, self.run) 25 | 26 | def create_model(self): 27 | return tf.keras.Sequential( 28 | [ 29 | tf.keras.layers.Dense(256, activation="elu"), 30 | tf.keras.layers.Dense(256, activation="elu"), 31 | tf.keras.layers.Dense(256, activation="elu"), 32 | tf.keras.layers.Dense(256, activation="elu"), 33 | tf.keras.layers.Dense(1, activation="sigmoid"), 34 | ] 35 | ) 36 | 37 | def run_one_step(self): 38 | observations = np.array(copy.deepcopy(self.get_cart_observations())) 39 | with tf.GradientTape() as tape: 40 | left_proba = self.model(observations[np.newaxis]) 41 | action = tf.random.uniform([1, 1]) > left_proba 42 | y_target = tf.constant([[1.0]]) - tf.cast(action, tf.float32) 43 | loss = tf.reduce_mean(self.loss_function(y_target, left_proba)) 44 | 45 | self.gradients.append(tape.gradient(loss, self.model.trainable_variables)) 46 | self.rewards.append(1.0) 47 | self.take_action(self.create_command(int(action))) 48 | self.step += self.reward 49 | 50 | def discount_rewards(self, rewards): 51 | discounted = np.array(rewards) 52 | for step in range(len(rewards) - 2, -1, -1): 53 | discounted[step] += discounted[step + 1] * self.discount_factor 54 | return discounted 55 | 56 | def discount_and_normalize_rewards(self): 57 | all_discounted_rewards = [self.discount_rewards(rewards) for rewards in self.episode_rewards] 58 | flat_rewards = np.concatenate(all_discounted_rewards) 59 | reward_mean = flat_rewards.mean() 60 | reward_std = flat_rewards.std() 61 | return [(discounted_rewards - reward_mean) / reward_std for discounted_rewards in all_discounted_rewards] 62 | 63 | def is_iteration_ended(self): 64 | return self.episode == self.max_number_of_episodes_per_iteration 65 | 66 | def clean_up_at_end_of_episode(self): 67 | self.episode_rewards.append(self.rewards) 68 | self.episode_gradients.append(self.gradients) 69 | self.rewards = [] 70 | self.gradients = [] 71 | 72 | def run(self): 73 | if not self.is_simulation_ready(): 74 | return 75 | elif self.iteration == self.max_number_of_iterations: 76 | quit() 77 | 78 | self.advance_episode_when_finished(self.clean_up_at_end_of_episode) 79 | self.advance_iteration_when_finished() 80 | self.run_one_step() 81 | 82 | def advance_iteration_when_finished(self): 83 | if self.is_iteration_ended(): 84 | self.get_logger().info( 85 | f"Ended {self.iteration} iteration with max final {max([sum(rewards) for rewards in self.episode_rewards])}" 86 | ) 87 | self.apply_gradients() 88 | self.episode_gradients = [] 89 | self.episode_rewards = [] 90 | self.episode = 0 91 | self.iteration += 1 92 | 93 | def apply_gradients(self): 94 | all_final_rewards = self.discount_and_normalize_rewards() 95 | all_mean_grads = self.calculate_mean_grads(all_final_rewards) 96 | self.optimizer.apply_gradients(zip(all_mean_grads, self.model.trainable_variables)) 97 | 98 | def calculate_mean_grads(self, all_final_rewards): 99 | all_mean_grads = [] 100 | for var_index in range(len(self.model.trainable_variables)): 101 | mean_grads = tf.reduce_mean( 102 | [ 103 | final_reward * self.episode_gradients[episode_index][step][var_index] 104 | for episode_index, final_rewards in enumerate(all_final_rewards) 105 | for step, final_reward in enumerate(final_rewards) 106 | ], 107 | axis=0, 108 | ) 109 | all_mean_grads.append(mean_grads) 110 | return all_mean_grads 111 | 112 | 113 | def main(args=None): 114 | rclpy.init(args=args) 115 | rclpy.spin(CartPoleReinforcementNeuralNetworkPolicy()) 116 | rclpy.shutdown() 117 | 118 | 119 | if __name__ == "__main__": 120 | main() 121 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/reinforcement_learning_node.py: -------------------------------------------------------------------------------- 1 | from std_msgs.msg import Float64 2 | from collections.abc import Callable 3 | from cart_pole_reinforcement_learning.cart_pole_learning_control_node import CartPoleReinforcementLearning 4 | from cart_pole_reinforcement_learning.cart_pole_reinforcement_learning_params import ( 5 | reinforcement_learning_node_parameters, 6 | ) 7 | 8 | 9 | class ReinforcementLearningNode(CartPoleReinforcementLearning): 10 | def __init__( 11 | self, 12 | name, 13 | max_number_of_episodes=None, 14 | max_number_of_steps=None, 15 | max_effort_command=None, 16 | discount_factor=None, 17 | reward=None, 18 | optimizer=None, 19 | loss_function=None, 20 | model=None, 21 | ): 22 | super().__init__(name) 23 | self.param_listener = reinforcement_learning_node_parameters.ParamListener(self) 24 | self.parameters = self.param_listener.get_params() 25 | self.max_number_of_episodes = ( 26 | self.parameters.max_number_of_episodes if max_number_of_episodes is None else max_number_of_episodes 27 | ) 28 | self.max_number_of_steps = ( 29 | self.parameters.max_number_of_steps if max_number_of_steps is None else max_number_of_steps 30 | ) 31 | self.max_effort_command = ( 32 | self.parameters.max_effort_command if max_effort_command is None else max_effort_command 33 | ) 34 | self.discount_factor = self.parameters.discount_factor if discount_factor is None else discount_factor 35 | self.reward = self.parameters.reward if max_number_of_steps is None else reward 36 | self.episode = 0 37 | self.step = 0 38 | self.optimizer = optimizer 39 | self.loss_function = loss_function 40 | self.model = model 41 | 42 | def is_episode_ended(self) -> bool: 43 | return self.step == self.max_number_of_steps 44 | 45 | def create_command(self, action: int) -> Float64: 46 | return Float64(data=self.max_effort_command) if action == 0 else Float64(data=-self.max_effort_command) 47 | 48 | def stop_run_when_learning_ended(self): 49 | if self.episode == self.max_number_of_episodes: 50 | quit() 51 | 52 | def advance_episode_when_finished(self, clean_up_function: Callable[[], None] = None): 53 | if self.is_episode_ended() or self.is_simulation_stopped(): 54 | self.get_logger().info(f"Ended episode: {self.episode} with score: {self.step}") 55 | self.episode += 1 56 | self.step = 0 57 | self.restart_learning_loop() 58 | clean_up_function and clean_up_function() 59 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/cart_pole_reinforcement_learning/reinforcement_learning_node_parameters.yaml: -------------------------------------------------------------------------------- 1 | reinforcement_learning_node_parameters: 2 | max_number_of_episodes: 3 | type: int 4 | default_value: 100 5 | read_only: true 6 | description: "Max number of episodes." 7 | max_number_of_steps: 8 | type: int 9 | default_value: 500 10 | read_only: true 11 | description: "Max number of steps." 12 | max_effort_command: 13 | type: double 14 | default_value: 5.0 15 | read_only: true 16 | description: "Effort command sent to cart." 17 | discount_factor: 18 | type: double 19 | default_value: 0.95 20 | read_only: true 21 | description: "Discount factor." 22 | reward: 23 | type: int 24 | default_value: 1 25 | read_only: true 26 | description: "Reward after each step." 27 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cart_pole_reinforcement_learning 5 | 1.0.0 6 | Reinforcement node and simple algorithms. 7 | Wiktor Bajor 8 | Apache-2.0 9 | 10 | generate_parameter_library 11 | rclpy 12 | python-tensorflow-pip 13 | python3-numpy 14 | std_srvs 15 | std_msgs 16 | cart_pole_observation_interface 17 | 18 | 19 | ament_python 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/resource/cart_pole_reinforcement_learning: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/cart_pole_reinforcement_learning/resource/cart_pole_reinforcement_learning -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/cart_pole_reinforcement_learning 3 | [install] 4 | install_scripts=$base/lib/cart_pole_reinforcement_learning 5 | -------------------------------------------------------------------------------- /src/cart_pole/cart_pole_reinforcement_learning/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | from generate_parameter_library_py.setup_helper import generate_parameter_module 3 | 4 | package_name = "cart_pole_reinforcement_learning" 5 | generate_parameter_module( 6 | "cart_pole_reinforcement_learning_params", 7 | "cart_pole_reinforcement_learning/reinforcement_learning_node_parameters.yaml", 8 | ) 9 | 10 | setup( 11 | name=package_name, 12 | version="1.0.0", 13 | packages=find_packages(exclude=["test"]), 14 | data_files=[ 15 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 16 | ("share/" + package_name, ["package.xml"]), 17 | ], 18 | install_requires=["setuptools"], 19 | zip_safe=True, 20 | maintainer="Wiktor Bajor", 21 | maintainer_email="wiktorbajor1@gmail.com", 22 | description="Reinforcement node and simple algorithms.", 23 | license="Apache-2.0", 24 | tests_require=["pytest"], 25 | entry_points={ 26 | "console_scripts": [ 27 | "cart_pole_basic_policy_node = cart_pole_reinforcement_learning.cart_pole_basic_policy_node:main", 28 | "cart_pole_neural_network_policy_node = cart_pole_reinforcement_learning.cart_pole_neural_network_policy:main", 29 | "cart_pole_deep_q_learning_policy_node = cart_pole_reinforcement_learning.cart_pole_deep_q_learning_policy_node:main", 30 | ], 31 | }, 32 | ) 33 | -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/genesis_simulations/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/genesis_simulations/genesis_simulations/__init__.py -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/genesis_simulations/cart_pole_basic_policy.py: -------------------------------------------------------------------------------- 1 | import genesis as gs 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | import random 5 | 6 | MAX_EPISODES = 100 7 | MAX_STEPS = 500 8 | EFFORT_CMD = 5.0 9 | MAX_POLE_ANGLE = 1.4 10 | MIN_POLE_ANGLE = -1.4 11 | MAX_CART_POSITION = 0.77 12 | MIN_CART_POSITION = -0.56 13 | 14 | 15 | def is_simulation_truncated(cart_position, pole_angle): 16 | return ( 17 | pole_angle >= MAX_POLE_ANGLE 18 | or pole_angle <= MIN_POLE_ANGLE 19 | or cart_position >= MAX_CART_POSITION 20 | or cart_position <= MIN_CART_POSITION 21 | ) 22 | 23 | 24 | def create_action(pole_angle): 25 | return EFFORT_CMD if pole_angle > 0 else -EFFORT_CMD 26 | 27 | 28 | def create_scene(): 29 | scene = gs.Scene( 30 | show_viewer=True, 31 | viewer_options=gs.options.ViewerOptions( 32 | camera_pos=(3.5, -1.0, 2.5), 33 | camera_lookat=(0.0, 0.0, 0.5), 34 | camera_fov=40, 35 | ), 36 | rigid_options=gs.options.RigidOptions( 37 | integrator=gs.integrator.Euler, 38 | ), 39 | sim_options=gs.options.SimOptions( 40 | dt=0.05, 41 | ), 42 | ) 43 | return scene 44 | 45 | 46 | def main(): 47 | gs.init(backend=gs.gpu, logging_level="error") 48 | scene = create_scene() 49 | scene.add_entity(gs.morphs.Plane()) 50 | cart_pole_urdf = os.path.join(get_package_share_directory("cart_pole_bringup"), "urdf", "cart_pole.urdf") 51 | cart_pole = scene.add_entity(gs.morphs.URDF(file=cart_pole_urdf, fixed=True)) 52 | scene.build() 53 | 54 | cart_joint_index = cart_pole.get_joint("slider_to_cart").dof_idx_local 55 | pole_joint_index = cart_pole.get_joint("slider_to_pole_with_holder").dof_idx_local 56 | 57 | for episode in range(MAX_EPISODES): 58 | scene.reset() 59 | reward = 0 60 | cart_pole.set_dofs_position([random.uniform(-0.05, 0.05)], [pole_joint_index]) 61 | observation = cart_pole.get_dofs_position() 62 | for _ in range(MAX_STEPS): 63 | cart_pole.control_dofs_force([create_action(observation[pole_joint_index])], [cart_joint_index]) 64 | scene.step() 65 | observation = cart_pole.get_dofs_position() 66 | reward += 1 67 | if is_simulation_truncated(observation[cart_joint_index], observation[pole_joint_index]): 68 | break 69 | 70 | print(f"Reward {reward} after {episode} episode") 71 | 72 | 73 | if __name__ == "__main__": 74 | main() 75 | -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | genesis_simulations 5 | 1.0.0 6 | Genesis simulation pkg. 7 | Wiktor Bajor 8 | Apache-2.0 9 | 10 | 11 | ament_python 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/resource/genesis_simulations: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wiktor-99/reinforcement_learning_playground/997ef109ba5d85abbc12ea464dcd20a3e234ff9a/src/cart_pole/genesis_simulations/resource/genesis_simulations -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/genesis_simulations 3 | [install] 4 | install_scripts=$base/lib/genesis_simulations 5 | -------------------------------------------------------------------------------- /src/cart_pole/genesis_simulations/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "genesis_simulations" 4 | 5 | setup( 6 | name=package_name, 7 | version="1.0.0", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="Wiktor Bajor", 16 | maintainer_email="wiktorbajor1@gmail.com", 17 | description="Genesis simulation pkg.", 18 | license="Apache-2.0", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": ["cart_pole_basic_policy_node = genesis_simulations.cart_pole_basic_policy:main"], 22 | }, 23 | ) 24 | -------------------------------------------------------------------------------- /src/cart_pole/simulation_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(simulation_control) 3 | 4 | add_compile_options(-Wall -Wextra -Wpedantic) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | find_package(std_srvs REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(rclcpp_components REQUIRED) 11 | 12 | find_package(gz_transport_vendor REQUIRED) 13 | find_package(gz-transport REQUIRED) 14 | 15 | find_package(gz_msgs_vendor REQUIRED) 16 | find_package(gz-msgs REQUIRED) 17 | 18 | add_library(simulation_control SHARED src/simulation_control_node.cpp) 19 | ament_target_dependencies(simulation_control rclcpp rclcpp_components std_srvs std_msgs) 20 | target_link_libraries(simulation_control gz-msgs::core gz-transport::core) 21 | rclcpp_components_register_node(simulation_control PLUGIN "SimulationControlNode" EXECUTABLE simulation_control_node) 22 | 23 | install(TARGETS 24 | simulation_control 25 | ARCHIVE DESTINATION lib 26 | LIBRARY DESTINATION lib 27 | RUNTIME DESTINATION bin) 28 | 29 | ament_package() 30 | -------------------------------------------------------------------------------- /src/cart_pole/simulation_control/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/cart_pole/simulation_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | simulation_control 5 | 1.0.0 6 | Package enables controlling simulation via ros2 service interface. 7 | Wiktor Bajor 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | gz_msgs_vendor 14 | gz_sim_vendor 15 | gz_transport_vendor 16 | std_msgs 17 | rclcpp_components 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/cart_pole/simulation_control/src/simulation_control_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | std::optional get_robot_description_from_topic() 12 | { 13 | const std::string topic_name{"robot_description"}; 14 | std::promise robot_description_promise; 15 | std::shared_future robot_description_future(robot_description_promise.get_future()); 16 | rclcpp::executors::SingleThreadedExecutor executor; 17 | auto ros2_node = std::make_shared("robot_description_acquire_node"); 18 | executor.add_node(ros2_node); 19 | const auto description_subs = ros2_node->create_subscription( 20 | topic_name, rclcpp::QoS(1).transient_local(), 21 | [&robot_description_promise](const std_msgs::msg::String::SharedPtr msg) 22 | { robot_description_promise.set_value(msg->data); }); 23 | 24 | rclcpp::FutureReturnCode future_ret; 25 | while (rclcpp::ok() && future_ret != rclcpp::FutureReturnCode::SUCCESS) 26 | { 27 | RCLCPP_INFO(ros2_node->get_logger(), "Waiting messages on topic [%s].", topic_name.c_str()); 28 | future_ret = executor.spin_until_future_complete(robot_description_future, std::chrono::seconds(1)); 29 | } 30 | 31 | if (future_ret != rclcpp::FutureReturnCode::SUCCESS) 32 | { 33 | RCLCPP_ERROR(ros2_node->get_logger(), "Failed to get XML from topic [%s].", topic_name.c_str()); 34 | return std::nullopt; 35 | } 36 | return robot_description_future.get(); 37 | } 38 | 39 | class SimulationControlNode : public rclcpp::Node 40 | { 41 | public: 42 | SimulationControlNode(const rclcpp::NodeOptions& options) : rclcpp::Node("simulation_control_node", options) 43 | { 44 | robot_description_ = *get_robot_description_from_topic(); 45 | server_ = create_service( 46 | "restart_sim_service", 47 | [&](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) 48 | { 49 | execute_gazebo_request(build_remove_request(), service_remove_); 50 | robot_name_ = std::string("cart_pole") + std::to_string(++counter_); 51 | execute_gazebo_request(build_create_request(), service_create_); 52 | rclcpp::sleep_for( 53 | std::chrono::milliseconds(500)); // Spawning model is unpredictable so arbitrary delay is used 54 | }); 55 | } 56 | 57 | gz::msgs::Entity build_remove_request() const 58 | { 59 | gz::msgs::Entity robot_remove_request; 60 | robot_remove_request.set_name(robot_name_); 61 | robot_remove_request.set_type(gz::msgs::Entity_Type_MODEL); 62 | 63 | return robot_remove_request; 64 | } 65 | 66 | gz::msgs::EntityFactory build_create_request() const 67 | { 68 | gz::msgs::EntityFactory robot_spawn_request; 69 | robot_spawn_request.set_sdf(robot_description_); 70 | robot_spawn_request.set_name(robot_name_); 71 | 72 | return robot_spawn_request; 73 | } 74 | 75 | template 76 | void execute_gazebo_request(T request, const std::string& service_name) 77 | { 78 | gz::msgs::Boolean response; 79 | bool result; 80 | const unsigned int timeout{5000}; 81 | while (rclcpp::ok() and not node_.Request(service_name, request, timeout, response, result)) 82 | { 83 | RCLCPP_WARN(this->get_logger(), "Waiting for service [%s] to become available ...", service_name.c_str()); 84 | } 85 | } 86 | 87 | gz::transport::Node node_{}; 88 | int counter_{}; 89 | std::string robot_name_{"cart_pole"}; 90 | std::string robot_description_{}; 91 | const std::string service_create_{"/world/empty/create"}; 92 | const std::string service_remove_{"/world/empty/remove"}; 93 | rclcpp::Service::SharedPtr server_; 94 | }; 95 | 96 | #include "rclcpp_components/register_node_macro.hpp" 97 | RCLCPP_COMPONENTS_REGISTER_NODE(SimulationControlNode) 98 | --------------------------------------------------------------------------------