diff --git a/.gitignore b/.gitignore index bdadc991f7..40ca370d21 100644 --- a/.gitignore +++ b/.gitignore @@ -206,7 +206,7 @@ bitbots_docs_internal # Neural Network Model Path /src/bitbots_vision/models/ -/src/bitbots_motion/bitbots_rl_motion/rl_walk_models/ +/src/bitbots_motion/bitbots_rl_motion/models/ **/.*. .idea/* diff --git a/pixi.lock b/pixi.lock index de0961dd72..1c5f379c37 100644 --- a/pixi.lock +++ b/pixi.lock @@ -2114,6 +2114,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/icu-78.2-h33c6efd_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/identify-2.6.17-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/importlib-metadata-8.7.0-pyhe01879c_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/jsoncpp-1.9.7-hea2e803_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/keyutils-1.6.3-hb9d3cd8_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/krb5-1.22.2-ha1258a1_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/ld_impl_linux-64-2.45.1-default_hbd61a6d_101.conda @@ -2147,6 +2148,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre-8.45-h9c3ff4c_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-64/pcre2-10.47-haa7fec5_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-64/perl-5.32.1-7_hd590300_perl5.conda + - conda: https://conda.anaconda.org/conda-forge/linux-64/pkg-config-0.29.2-h4bc722e_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/platformdirs-4.9.4-pyhcf101f3_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pre-commit-4.5.1-pyha770c72_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pycparser-2.22-pyh29332c3_1.conda @@ -2186,6 +2188,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/icu-78.2-hcab7f73_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/identify-2.6.17-pyhd8ed1ab_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/importlib-metadata-8.7.0-pyhe01879c_1.conda + - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/jsoncpp-1.9.7-h97bce4b_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/keyutils-1.6.3-h86ecc28_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/krb5-1.22.2-hfd895c2_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/ld_impl_linux-aarch64-2.45.1-default_h1979696_101.conda @@ -2197,6 +2200,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libffi-3.5.2-h376a255_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgcc-15.2.0-h8acb6b2_18.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgcc-ng-15.2.0-he9431aa_18.conda + - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libglib-2.86.4-hf53f6bf_1.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libgomp-15.2.0-h8acb6b2_18.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libiconv-1.18-h90929bb_2.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/libllvm21-21.1.8-hfd2ba90_0.conda @@ -2219,6 +2223,7 @@ environments: - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pcre-8.45-h01db608_0.tar.bz2 - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pcre2-10.47-hf841c20_0.conda - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/perl-5.32.1-7_h31becfc_perl5.conda + - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/pkg-config-0.29.2-hce167ba_1009.conda - conda: https://conda.anaconda.org/conda-forge/noarch/platformdirs-4.9.4-pyhcf101f3_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pre-commit-4.5.1-pyha770c72_0.conda - conda: https://conda.anaconda.org/conda-forge/noarch/pycparser-2.22-pyh29332c3_1.conda @@ -8848,6 +8853,16 @@ packages: purls: [] size: 169093 timestamp: 1733780223643 +- conda: https://conda.anaconda.org/conda-forge/linux-64/jsoncpp-1.9.7-hea2e803_0.conda + sha256: beed26e7105ec10e285a975a4310941fb1810e6d443b2b81a30836310f638b32 + md5: b3635009f823a05195a6e803da0fc90b + depends: + - __glibc >=2.17,<3.0.a0 + - libgcc >=14 + - libstdcxx >=14 + license: LicenseRef-Public-Domain OR MIT + size: 172034 + timestamp: 1773978619936 - conda: https://conda.anaconda.org/conda-forge/linux-aarch64/jsoncpp-1.9.6-h34915d9_1.conda sha256: 12f2d001e4e9ad255f1de139e873876d03d53f16396d73f7849b114eefec5291 md5: 2f23d5c1884fac280816ac2e5f858a65 @@ -8858,6 +8873,15 @@ packages: purls: [] size: 162312 timestamp: 1733779925983 +- conda: https://conda.anaconda.org/conda-forge/linux-aarch64/jsoncpp-1.9.7-h97bce4b_0.conda + sha256: 3357ae5511ec8e4a07d1b53132ff160f629f9f43e342eb9ccb2a9f4ee83400de + md5: 3c74fa5a2e13e30ef99efd2192b314d9 + depends: + - libgcc >=14 + - libstdcxx >=14 + license: LicenseRef-Public-Domain OR MIT + size: 163574 + timestamp: 1773978566040 - pypi: https://files.pythonhosted.org/packages/4f/9a/ab96291470e305504aa4b7a2e0ec132e930da89eb3ca7a82fbe03167c131/jsonlines-1.2.0-py2.py3-none-any.whl name: jsonlines version: 1.2.0 diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py index 6ed79c92b8..2dab8170df 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py +++ b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py @@ -1,6 +1,7 @@ from enum import Flag from typing import Optional +from geometry_msgs.msg import PoseStamped from rclpy.action import ActionClient from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.duration import Duration @@ -34,6 +35,7 @@ class WalkKickTargets(Flag): RIGHT = True walk_kick_pub: Publisher + rl_kick_pub: Publisher def __init__(self, node, blackboard): super().__init__(node, blackboard) @@ -42,6 +44,7 @@ def __init__(self, node, blackboard): """ self.walk_kick_pub = self._node.create_publisher(Bool, "/kick", 1) # self.connect_dynamic_kick() Do not connect if dynamic_kick is disabled + self.rl_kick_pub = self._node.create_publisher(PoseStamped, "/rl_command/kick_command", 1) def walk_kick(self, target: WalkKickTargets) -> None: """ @@ -81,6 +84,13 @@ def dynamic_kick(self, goal: Kick.Goal) -> None: self.last_goal = goal self.last_goal_sent = self._node.get_clock().now() + def rl_kick(self, ball_pose: PoseStamped) -> None: + """ + Kick the ball using the RL kick + :param goal_pose: Pose to kick to + """ + self.rl_kick_pub.publish(ball_pose) + def __feedback_cb(self, feedback): self.last_feedback: Kick.Feedback = feedback.feedback self.last_feedback_received = self._node.get_clock().now() diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py index c8743160aa..c4d4d10dc1 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py @@ -2,6 +2,7 @@ from bitbots_blackboard.capsules.kick_capsule import KickCapsule from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from geometry_msgs.msg import PoseStamped from bitbots_msgs.action import Kick @@ -123,3 +124,33 @@ def perform(self, reevaluate=False): self._goal_sent = True else: self.pop() + + +# TODO: Fix integration +class RLKick(AbstractKickAction): + def __init__(self, blackboard, dsd, parameters): + super().__init__(blackboard, dsd, parameters) + + self.kick_length = self.blackboard.config["kick_cost_kick_length"] + self.angular_range = self.blackboard.config["kick_cost_angular_range"] + self.max_kick_angle = self.blackboard.config["max_kick_angle"] + self.num_kick_angles = self.blackboard.config["num_kick_angles"] + self.penalty_kick_angle = self.blackboard.config["penalty_kick_angle"] + + def perform(self, reevaluate=False): + ball_pose = PoseStamped() + ball_pose.header.stamp = self.blackboard.node.get_clock().now().to_msg() + ball_pose.header.frame_id = self.blackboard.world_model.base_footprint_frame + + ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv() + ball_pose.pose.position.x = ball_u + ball_pose.pose.position.y = ball_v + ball_pose.pose.position.z = 0.0 + + ball_pose.pose.orientation.x = 0.0 # isn't used + ball_pose.pose.orientation.y = 0.0 + ball_pose.pose.orientation.z = 0.0 + ball_pose.pose.orientation.w = 1.0 + + self.blackboard.kick.rl_kick(ball_pose) + self.pop() diff --git a/src/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml b/src/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml index d227fab9c6..6b23ecf276 100644 --- a/src/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml +++ b/src/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml @@ -25,7 +25,7 @@ wolfgang_hardware_interface: servos: # specifies which information should be read read_position: true - read_velocity: false + read_velocity: true read_effort: false read_pwm: false read_volt_temp: true # this also corresponds for the error byte @@ -61,7 +61,7 @@ wolfgang_hardware_interface: Position_D_Gain: 0 #2800 # [/16] 0~16,383 Position_I_Gain: 0 #180000 # [/ 65,536] 0~16,383 #If robot starts to tremble, reduce Position_P_Gain - Position_P_Gain: 1200 #1100 # [/ 128] 0~16,383 + Position_P_Gain: 300 #1100 # [/ 128] 0~16,383 Feedforward_2nd_Gain: 0 # [/4] Feedforward_1st_Gain: 0 # [/4] Profile_Acceleration: 0 # 0 for infinite @@ -89,7 +89,7 @@ wolfgang_hardware_interface: Position_D_Gain: 0 #2800 # [/16] 0~16,383 Position_I_Gain: 0 #180000 # [/ 65,536] 0~16,383 #If robot starts to tremble, reduce Position_P_Gain - Position_P_Gain: 800 #1100 # [/ 128] 0~16,383 + Position_P_Gain: 1100 #1100 # [/ 128] 0~16,383 Feedforward_2nd_Gain: 0 # [/4] Feedforward_1st_Gain: 0 # [/4] Profile_Acceleration: 0 # 0 for infinite @@ -117,7 +117,7 @@ wolfgang_hardware_interface: Position_D_Gain: 0 #2800 # [/16] 0~16,383 Position_I_Gain: 0 #180000 # [/ 65,536] 0~16,383 #If robot starts to tremble, reduce Position_P_Gain - Position_P_Gain: 4000 #1100 # [/ 128] 0~16,383 + Position_P_Gain: 600 #1100 # [/ 128] 0~16,383 Feedforward_2nd_Gain: 0 # [/4] Feedforward_1st_Gain: 0 # [/4] Profile_Acceleration: 0 # 0 for infinite @@ -145,7 +145,7 @@ wolfgang_hardware_interface: Position_D_Gain: 0 #2800 # [/16] 0~16,383 Position_I_Gain: 0 #180000 # [/ 65,536] 0~16,383 #If robot starts to tremble, reduce Position_P_Gain - Position_P_Gain: 500 #1100 # [/ 128] 0~16,383 + Position_P_Gain: 100 #1100 # [/ 128] 0~16,383 Feedforward_2nd_Gain: 0 # [/4] Feedforward_1st_Gain: 0 # [/4] Profile_Acceleration: 0 # 0 for infinite diff --git a/src/bitbots_misc/bitbots_bringup/launch/motion.launch b/src/bitbots_misc/bitbots_bringup/launch/motion.launch index d766d93758..f0b0648a39 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/motion.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/motion.launch @@ -2,7 +2,8 @@ - + + @@ -38,7 +39,7 @@ - + @@ -58,6 +59,12 @@ --> + + + + + + diff --git a/src/bitbots_misc/bitbots_bringup/package.xml b/src/bitbots_misc/bitbots_bringup/package.xml index f3aec43bbb..57a6d6d87d 100644 --- a/src/bitbots_misc/bitbots_bringup/package.xml +++ b/src/bitbots_misc/bitbots_bringup/package.xml @@ -28,6 +28,7 @@ bitbots_localization bitbots_odometry bitbots_quintic_walk + bitbots_rl_motion bitbots_robot_description bitbots_ros_control bitbots_utils diff --git a/src/bitbots_motion/bitbots_dynup/config/dynup_config.yaml b/src/bitbots_motion/bitbots_dynup/config/dynup_config.yaml index aeef4f8a29..1450644cbc 100644 --- a/src/bitbots_motion/bitbots_dynup/config/dynup_config.yaml +++ b/src/bitbots_motion/bitbots_dynup/config/dynup_config.yaml @@ -33,21 +33,21 @@ bitbots_dynup: trunk_height: type: double description: "End pose trunk height. Depends on walkready of walking." - default_value: 0.38 + default_value: 0.372 validation: bounds<>: [0.0, 1.0] trunk_pitch: type: double description: "End pose trunk pitch. Depends on walkready of walking." - default_value: 0.2 + default_value: 11.459 validation: - bounds<>: [0.0, 1.0] + bounds<>: [-40.0, 40.0] trunk_x_final: type: double description: "End pose position of the trunk in x direction. Depends on walkready of walking." - default_value: 0.001 + default_value: -0.0839 validation: - bounds<>: [0.0, 1.0] + bounds<>: [-0.2, 0.2] arm_side_offset_back: type: double description: "End pose arm side offset back. Depends on walkready of walking." diff --git a/src/bitbots_motion/bitbots_rl_motion/CMakeLists.txt b/src/bitbots_motion/bitbots_rl_motion/CMakeLists.txt deleted file mode 100644 index bc44f75a6d..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(bitbots_rl_motion) - -find_package(ament_cmake REQUIRED) -find_package(bitbots_docs REQUIRED) - -enable_bitbots_docs() - -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/rl_walk_models") - install(DIRECTORY rl_walk_models DESTINATION share/${PROJECT_NAME}) -endif() -install(PROGRAMS scripts/rl_walk.py DESTINATION lib/${PROJECT_NAME}) - -ament_package() diff --git a/src/bitbots_motion/bitbots_rl_motion/README.md b/src/bitbots_motion/bitbots_rl_motion/README.md new file mode 100644 index 0000000000..500c45c74d --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/README.md @@ -0,0 +1,27 @@ +## General + +The package contains a framework which is a capsule for the application of policies on a robot. + +## Framework structure + +The code is divided in five sections: Configs, Handlers, Nodes, Launch and rest. + +The Nodes-folder contains all relevant ROS-Nodes regarding policy models. These nodes are responsible for starting the policies correctly, feeding them with correct data and publishing their outputs correctly. +The name of the node describes for which kind of policy it is suitable. +The RL Node is a special case. All other nodes are kids of the RL Node. It centralizes the execution loop and minimizes boiler plate code. + +The Handlers-folder contains all handlers. A handler is a specific type of object which is responsible for processing external data such that they are comprehensible for the policy models. All handlers are kids of the Handler class. + +The Configs-folder contains all robot/policy specific configurations. Files in the Configs-folder should be in .yaml-format. They also contain the paths to the onnx-policy models. + +The Launch-folder contains a launch file which starts all relevant policy nodes. + +phase.py and previous_action.py are two files, which do not fall in any of the aforementioned categories. +phase.py defines a PhaseObject, which is responsible for the phase management. previous_action.py defindes a PreviousAction object, which is responsible for saving and provide the previous action. +Both files are located in the bitbots_rl_motion folder. + +## Execution + +For proper starting you need a policy model and a config file. The config file should have the same structure as the wolfgang_dribbling_model_config.yaml file. +Furthermore, you have to create or adjust a node file to your needs. walk_node.py can be used for orientation. If chages are conducted on the RL_Node class, it should be announced. +Finally, you define which nodes and policies you wanna use in the launch file. diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/__init__.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py new file mode 100644 index 0000000000..8b9034dfb5 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py @@ -0,0 +1,37 @@ +import numpy as np +from rclpy.node import Node + +# Please pay attention to the code in rl_node.py if you wanna change here sth. + + +class PhaseObject(Node): + _phase: np.ndarray = np.array([0.0, np.pi], dtype=np.float32) + _phase_dt: float + + def __init__(self, config): + if self.config["phase"]: + self._control_dt = config["phase"]["control_dt"] + self._gait_frequency = config["phase"]["gait_frequency"] + self._phase_dt = 2 * np.pi * self._gait_frequency * self._control_dt + else: + self._control_dt = None + self._gait_frequency = None + self._phase_dt = None + self.get_logger().warning("No phase was found! Using policy without phase!") + + self._obs_phase = None + + def set_phase(self, new_phase): + self._phase = new_phase + + def set_obs_phase(self, new_obs_phase): + self._obs_phase = new_obs_phase + + def get_phase(self): + return self._phase + + def get_phase_dt(self): + return self._phase_dt + + def get_obs_phase(self): + return self._obs_phase diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/previous_action.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/previous_action.py new file mode 100644 index 0000000000..fba1ec7ab0 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/previous_action.py @@ -0,0 +1,14 @@ +import numpy as np + + +class PreviousActionObject: + def __init__(self, config): + self._previous_action: np.ndarray = np.zeros( + len(config["joints"]["ordered_relevant_joint_names"]), dtype=np.float32 + ) + + def set_previous_action(self, new_previous_action): + self._phase = new_previous_action + + def get_previous_action(self): + return self._previous_action diff --git a/src/bitbots_motion/bitbots_rl_motion/config/rl_walk_sim.yaml b/src/bitbots_motion/bitbots_rl_motion/config/rl_walk_sim.yaml deleted file mode 100644 index 6d483f84e4..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/config/rl_walk_sim.yaml +++ /dev/null @@ -1,3 +0,0 @@ -rl_walk: - ros__parameters: - model_folder: 'webots_neu' diff --git a/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_dribbling_model_config.yaml b/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_dribbling_model_config.yaml new file mode 100644 index 0000000000..18b0ade6c9 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_dribbling_model_config.yaml @@ -0,0 +1,38 @@ +model: wolfgang_dribbling_ppo.onnx + +joints: + ordered_relevant_joint_names: [ + "RHipYaw", + "RHipRoll", + "RHipPitch", + "RKnee", + "RAnklePitch", + "RAnkleRoll", + "LHipYaw", + "LHipRoll", + "LHipPitch", + "LKnee", + "LAnklePitch", + "LAnkleRoll", + ] + + walkready_state: [ + 0.023628265148262724, + -0.10401795710581162, + -0.7352626990449959, + -1.3228415184260092, + 0.5495038397740458, + -0.12913515511895796, + -0.016441795868928723, + 0.07253788412595062, + 0.7420808433462046, + 1.334527650998329, + -0.5537397918567754, + 0.07437380704149316, + ] + +phase: + control_dt: 0.02 + gait_frequency: 1.5 + +providers: ["CPUExecutionProvider"] diff --git a/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_walking_model_config.yaml b/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_walking_model_config.yaml new file mode 100644 index 0000000000..97c7bf3653 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/configs/wolfgang_walking_model_config.yaml @@ -0,0 +1,38 @@ +model: wolfgang_walk_ppo.onnx + +joints: + ordered_relevant_joint_names: [ + "RHipYaw", + "RHipRoll", + "RHipPitch", + "RKnee", + "RAnklePitch", + "RAnkleRoll", + "LHipYaw", + "LHipRoll", + "LHipPitch", + "LKnee", + "LAnklePitch", + "LAnkleRoll", + ] + + walkready_state: [ + 0.023628265148262724, + -0.10401795710581162, + -0.7352626990449959, + -1.3228415184260092, + 0.5495038397740458, + -0.12913515511895796, + -0.016441795868928723, + 0.07253788412595062, + 0.7420808433462046, + 1.334527650998329, + -0.5537397918567754, + 0.07437380704149316, + ] + +phase: + control_dt: 0.02 + gait_frequency: 1.5 + +providers: ["CPUExecutionProvider"] diff --git a/src/bitbots_motion/bitbots_rl_motion/docs/_static/logo.png b/src/bitbots_motion/bitbots_rl_motion/docs/_static/logo.png deleted file mode 100644 index f8afdd5d06..0000000000 Binary files a/src/bitbots_motion/bitbots_rl_motion/docs/_static/logo.png and /dev/null differ diff --git a/src/bitbots_motion/bitbots_rl_motion/docs/conf.py b/src/bitbots_motion/bitbots_rl_motion/docs/conf.py deleted file mode 100644 index 0b6342a9ce..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/docs/conf.py +++ /dev/null @@ -1,187 +0,0 @@ -# -# Full list of options at http://www.sphinx-doc.org/en/master/config - -# -- Path setup -------------------------------------------------------------- - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys - -import catkin_pkg.package -from exhale import utils - -package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) -) -sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) - - -# -- Helper functions -------------------------------------------------------- - - -def count_files(): - """:returns tuple of (num_py, num_cpp)""" - num_py = 0 - num_cpp = 0 - - for _, _, files in os.walk(os.path.join(package_dir, "src")): - for f in files: - if f.endswith(".py"): - num_py += 1 - for _, _, files in os.walk(os.path.join(package_dir, "include")): - for f in files: - if f.endswith(".h") or f.endswith(".hpp"): - num_cpp += 1 - - return num_py, num_cpp - - -# -- Project information ----------------------------------------------------- - -project = catkin_package.name -copyright = "2019, Bit-Bots" -author = ", ".join([a.name for a in catkin_package.authors]) - -# The short X.Y version -version = str(catkin_package.version) -# The full version, including alpha/beta/rc tags -release = str(catkin_package.version) - -# -- General configuration --------------------------------------------------- - -# If your documentation needs a minimal Sphinx version, state it here. -# -# needs_sphinx = '1.0' - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = [ - "sphinx.ext.autodoc", - "sphinx.ext.doctest", - "sphinx.ext.intersphinx", - "sphinx.ext.todo", - "sphinx.ext.coverage", - "sphinx.ext.imgmath", - "sphinx.ext.viewcode", - "sphinx_rtd_theme", -] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ["_templates"] - -# The suffix(es) of source filenames. -# You can specify multiple suffix as a list of string: -# -# source_suffix = ['.rst', '.md'] -source_suffix = ".rst" - -# The master toctree document. -master_doc = "index" - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -# -# This is also used if you do content translation via gettext catalogs. -# Usually you set "language" from the command line for these cases. -language = None - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = None - -# -- Exhale and Breath setup ------------------------------------------------- - -# Tell sphinx what the primary language being documented is. -num_files_py, num_files_cpp = count_files() -primary_domain = "py" if num_files_py >= num_files_cpp else "cpp" - -# Tell sphinx what the pygments highlight language should be. -highlight_language = primary_domain - -if num_files_cpp > 0: - extensions += [ - "breathe", - "exhale", - ] - - breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} - breathe_default_project = project - - def specifications_for_kind(kind): - # Show all members for classes and structs - if kind == "class" or kind == "struct": - return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] - # An empty list signals to Exhale to use the defaults - else: - return [] - - exhale_args = { - # These arguments are required - "containmentFolder": "cppapi", - "rootFileName": "library_root.rst", - "rootFileTitle": "C++ Library API", - "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), - # Suggested optional arguments - "createTreeView": True, - "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), - } - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = "sphinx_rtd_theme" - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ["_static"] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# -# The default sidebars (for documents that don't match any pattern) are -# defined by theme itself. Builtin themes are using these templates by -# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', -# 'searchbox.html']``. -# -# html_sidebars = {} - -html_logo = os.path.join("_static", "logo.png") -html_favicon = os.path.join("_static", "logo.png") - - -# -- Options for intersphinx extension --------------------------------------- - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {"https://docs.python.org/": None} - -# -- Options for todo extension ---------------------------------------------- - -# If true, `todo` and `todoList` produce output, else they produce nothing. -todo_include_todos = True - -# -- RST Standard variables --------------------------------------------------- -rst_prolog = f".. |project| replace:: {project}\n" -rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format( - ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" -) diff --git a/src/bitbots_motion/bitbots_rl_motion/docs/index.rst b/src/bitbots_motion/bitbots_rl_motion/docs/index.rst deleted file mode 100644 index e76aa433a2..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/docs/index.rst +++ /dev/null @@ -1,21 +0,0 @@ -Welcome to |project|'s documentation! -================================================ - -Description ------------ - -|description| - -.. toctree:: - :maxdepth: 2 - - cppapi/library_root - pyapi/modules - - -Indices and tables -================== - -* :ref:`genindex` -* |modindex| -* :ref:`search` diff --git a/src/bitbots_motion/bitbots_rl_motion/docs/manual/rl_motion.rst b/src/bitbots_motion/bitbots_rl_motion/docs/manual/rl_motion.rst deleted file mode 100644 index 3801c65201..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/docs/manual/rl_motion.rst +++ /dev/null @@ -1,16 +0,0 @@ -RL Motion -========= - -This package provides ROS nodes that execute motions that were previously learned via reinforcement learning. -The node uses model from the 'models' folder based on the config in the 'config'. - -Using a New Model ------------------ - -The model can be trained using the code from the https://github.com/bit-bots/deep_quintic git. -The training script will generate a folder in the specified log location. -You need to choose which saved model (either the best model or one of the checkpoints) should be used. -Rename the corresponding file to 'model.zip'. -Copy it to a new folder in the 'models' folder together with the 'args.yml', 'config.yml' and vecnormalize.pkl'. -Change the config to point to your new model. -See the documentation in the git for more information on training models. \ No newline at end of file diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/__init__.py b/src/bitbots_motion/bitbots_rl_motion/handlers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/ball_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/ball_handler.py new file mode 100644 index 0000000000..96c657a1b7 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/ball_handler.py @@ -0,0 +1,30 @@ +import numpy as np +from soccer_vision_3d_msgs.msg import BallArray + +from handlers.handler import Handler + + +class BallHandler(Handler): + def __init__(self, config): + super().__init__(config) + + self._ball_pos = None + + self._ball_pos_sub = self.create_subscription(BallArray, "balls_relative", self._ball_pos_callback, 10) + + def _ball_pos_callback(self, msg): + if msg.balls: + self._ball_pos = msg.balls[0].center + + def has_data(self): + return self._ball_pos is not None + + def get_ball_pos(self): + ball_pos = np.array( + [ + self._ball_pos.x, + self._ball_pos.y, + ], + dtype=np.float32, + ) + return ball_pos diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py new file mode 100644 index 0000000000..ae034e8473 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py @@ -0,0 +1,23 @@ +import numpy as np +from geometry_msgs.msg import Twist + +from handlers.handler import Handler + + +class CommandHandler(Handler): + def __init__(self, config): + super().__init__(config) + + self._cmd_vel = None + + self._cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self._cmd_vel_callback, 10) + + def get_command(self): + command = np.array([self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z], dtype=np.float32) + return command + + def has_data(self): + return self._cmd_vel is not None + + def _cmd_vel_callback(self, msg): + self._cmd_vel = msg diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py new file mode 100644 index 0000000000..bca7c224c5 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py @@ -0,0 +1,36 @@ +import numpy as np +from sensor_msgs.msg import Imu +from transforms3d.euler import euler2mat +from transforms3d.quaternions import quat2mat + +from handlers.handler import Handler + + +class GravityHandler(Handler): + def __init__(self, config): + super().__init__(config) + + self._imu_data = None + + self._imu_sub = self.create_subscription(Imu, "imu/data", self._imu_callback, 10) + + # Callables + def _imu_callback(self, msg): + self._imu_data = msg + + def has_data(self): + return self._imu_data is not None + + def get_gravity(self): + gravity = ( + quat2mat( + [ + self._imu_data.orientation.w, + self._imu_data.orientation.x, + self._imu_data.orientation.y, + self._imu_data.orientation.z, + ] + ) + @ euler2mat(0, -0.0, 0) + ).T @ np.array([0, 0, -1], dtype=np.float32) + return gravity diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/gyro_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/gyro_handler.py new file mode 100644 index 0000000000..32417e5b4c --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/gyro_handler.py @@ -0,0 +1,32 @@ +import numpy as np +from sensor_msgs.msg import Imu + +from handlers.handler import Handler + + +class GyroHandler(Handler): + def __init__(self, config): + super().__init__(config) + + self._imu_data = None + + self._imu_sub = self.create_subscription(Imu, "imu/data", self._imu_callback, 10) + + # Callables + def _imu_callback(self, msg): + self._imu_data = msg + + def has_data(self): + return self._imu_data is not None + + def get_gyro(self): + gyro = np.array( + [ + self._imu_data.angular_velocity.x, + self._imu_data.angular_velocity.y, + self._imu_data.angular_velocity.z, + ], + dtype=np.float32, + ) + + return gyro diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/handler.py new file mode 100644 index 0000000000..36201ba6c0 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/handler.py @@ -0,0 +1,12 @@ +from abc import ABC, abstractmethod + +from rclpy.node import Node + + +class Handler(ABC, Node): + def __init__(self, config): + self._config = config + + @abstractmethod + def has_data(self): + pass diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py new file mode 100644 index 0000000000..845c1e8d5c --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py @@ -0,0 +1,76 @@ +import numpy as np +from sensor_msgs.msg import JointState + +from bitbots_msgs.msg import JointCommand +from handlers.handler import Handler + + +class JointHandler(Handler): + def __init__(self, config): + super().__init__(config) + + self._ordered_relevant_joint_names = self._config["joints"]["ordered_relevant_joint_names"] + self._walkready_state = self._config["joints"]["walkready_state"] + self._previous_action: np.ndarray = np.zeros(len(self._ordered_relevant_joint_names), dtype=np.float32) + self._joint_state = None + + self._joint_state_sub = self.create_subscription(JointState, "joint_states", self._joint_state_callback, 10) + + def _joint_state_callback(self, msg): + self._joint_state = msg + + def has_data(self): + return self._joint_state is not None + + def get_angle_data(self): + joint_angles = ( + np.array( + [ + self._joint_state.position[self._joint_state.name.index(name)] + for name in self._ordered_relevant_joint_names + ], + dtype=np.float32, + ) + - self._walkready_state + ) + + return joint_angles + + def get_velocity_data(self): + joint_velocities = np.array( + [ + self._joint_state.velocity[self._joint_state.name.index(name)] + for name in self._ordered_relevant_joint_names + ], + dtype=np.float32, + ) + + return joint_velocities + + def get_data(self): + return self.get_angle_data(), self.get_velocity_data() + + def get_walkready_joint_command(self, timestamp): + joint_command = JointCommand() + joint_command.joint_names = self._ordered_relevant_joint_names + joint_command.velocities = [0.2] * len(self._ordered_relevant_joint_names) + joint_command.accelerations = [-1.0] * len(self._ordered_relevant_joint_names) + joint_command.max_currents = [-1.0] * len(self._ordered_relevant_joint_names) # -1.0 means no limit + joint_command.header.stamp = timestamp + joint_command.positions = self._walkready_state + + return joint_command + + def get_joint_commands(self, onnx_pred): + joint_command = JointCommand() + joint_command.header.stamp = self._joint_state.header.stamp + joint_command.joint_names = self._ordered_relevant_joint_names + joint_command.positions = onnx_pred * 0.5 + self._walkready_state + joint_command.velocities = [-1.0] * len(self._ordered_relevant_joint_names) + joint_command.accelerations = [-1.0] * len(self._ordered_relevant_joint_names) + joint_command.max_currents = [-1.0] * len(self._ordered_relevant_joint_names) + + return joint_command + + def get_previous_action_initial(self): + return self._previous_action diff --git a/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch b/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch new file mode 100644 index 0000000000..97f3a6ee92 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/bitbots_motion/bitbots_rl_motion/launch/rl_walk.launch b/src/bitbots_motion/bitbots_rl_motion/launch/rl_walk.launch deleted file mode 100644 index 08028179c5..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/launch/rl_walk.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/bitbots_motion/bitbots_rl_motion/launch/test.launch b/src/bitbots_motion/bitbots_rl_motion/launch/test.launch deleted file mode 100644 index 638d8babd2..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/launch/test.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_dribbling_ppo.onnx b/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_dribbling_ppo.onnx new file mode 100644 index 0000000000..c9c06e19f8 Binary files /dev/null and b/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_dribbling_ppo.onnx differ diff --git a/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_walk_ppo.onnx b/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_walk_ppo.onnx new file mode 100644 index 0000000000..ab14b6de0e Binary files /dev/null and b/src/bitbots_motion/bitbots_rl_motion/models/wolfgang_walk_ppo.onnx differ diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/__init__.py b/src/bitbots_motion/bitbots_rl_motion/nodes/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/kick_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/kick_node.py new file mode 100644 index 0000000000..acbd1d927a --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/kick_node.py @@ -0,0 +1,52 @@ +import numpy as np +from handlers.ball_handler import BallHandler +from handlers.gravity_handler import GravityHandler +from handlers.gyro_handler import GyroHandler +from handlers.joint_handler import JointHandler + +from bitbots_msgs.msg import JointCommand +from nodes.rl_node import RLNode + + +class KickNode(RLNode): + def __init__(self, config_path: str): + # Configuring self._config, self._phase, self._previous_action + super().__init__(config_path, node_name="kick_node") + + # publishers + self._joint_command_pub = self.create_publisher(JointCommand, "kick_motor_goals", 10) + + # handlers + self._gyro_handler = GyroHandler(self._config) + self._gravity_handler = GravityHandler(self._config) + self._joint_handler = JointHandler(self._config) + self._ball_handler = BallHandler(self._config) + + # loading model + model = self._config["model"] + self.load_model(model) + + # observations + def obs(self): + observation = np.hstack( + [ + self._gyro_handler.get_gyro(), + self._gravity_handler.get_gravity(), + self._joint_handler.get_velocity_data(), + self._joint_handler.get_angle_data(), + self._previous_action.get_previous_action(), + self._phase.get_obs_phase(), + self._ball_handler.get_ball_pos(), + ] + ).astype(np.float32) + + return observation + + # load phase function + def load_phase(self): + pass + + # publisher function + def publisher(self, onnx_pred): + joint_command = self._joint_handler.get_joint_commands(onnx_pred) + self._joint_command_pub.publish(joint_command) diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py new file mode 100644 index 0000000000..0870b7a59b --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py @@ -0,0 +1,133 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Deploy an MJX policy in ONNX format to C MuJoCo and play with it.""" + +import os +from abc import ABC, abstractmethod +from pathlib import Path + +import numpy as np +import onnx +import onnxruntime as rt +import yaml +from ament_index_python import get_package_share_directory +from bitbots_rl_motion.phase import PhaseObject +from bitbots_rl_motion.previous_action import PreviousActionObject +from handlers.handler import Handler +from rclpy.node import Node +from rclpy.subscription import Subscription + + +class RLNode(Node, ABC): + """Node to control the wolfgang humanoid.""" + + def __init__(self, config_path: str, node_name: str): + super().__init__(f"{node_name}") + + self._config = self._load_config(config_path) + # Phase is optional - if phase shouldn't be used, than self._phase.get_phase() will return None + self._phase = PhaseObject(self._config) + self._previous_action = PreviousActionObject(self._config) + + def _load_config(self, path: str): + with open(path) as f: + return yaml.safe_load(f) + + def _timer_callback(self): + if not self._config: + raise ConfigError("Configuration is missing!") + + # Prüfen ob alle Subscriber schon mindestens eine Nachricht hatten + + sensors_ready, missing_handler = self._all_sensors_ready() + if not sensors_ready: + self.get_logger().warning( + f"Waiting for all sensors to be available. Following handler hasn't got the needed information: {missing_handler}", + throttle_duration_sec=1.0, + ) + return + else: + self.get_logger().info("All sensors are available!") + + # TODO consider IMU mounting offset + + if self._phase.get_phase(): + self._phase.set_obs_phase( + np.array( + [np.cos(self._phase.get_phase()), np.sin(self._phase.get_phase())], + dtype=np.float32, + ).flatten() + ) + + observation = self.obs() + + # Run the ONNX model + onnx_input = {self._onnx_input_name[0]: observation.reshape(1, -1)} # TODO: Improve input + onnx_pred = self._onnx_session.run(self._onnx_output_name, onnx_input)[0][0] + self._previous_action.set_previous_action(onnx_pred) + + self.publisher(onnx_pred) + + if self._phase.get_phase(): + phase_tp1 = self._phase.get_phase() + self._phase.get_phase_dt() + self._phase.set_phase(np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi) + + def _all_sensors_ready(self): + for handler in self._handlers: + if not handler.has_data(): + return False, type(handler).__name__ + + return True, "No missing handler" + + def load_model(self, model): + path_to_model = os.path.join(get_package_share_directory("bitbots_rl_motion"), "models", model) + + self._onnx_model_path = Path(path_to_model) + + # Load the ONNX model + self._onnx_session = rt.InferenceSession(self._onnx_model_path, providers=self._config["providers"]) + self._onnx_model = onnx.load(self._onnx_model_path) + + self._onnx_input_name = [inp.name for inp in self._onnx_model.graph.input] + self._onnx_output_name = [out.name for out in self._onnx_model.graph.output] + + self._subs = [] + self._handlers = [] + + for _, value in self.__dict__.items(): + if isinstance(value, Subscription): + self._subs.append(value) + if isinstance(value, Handler): + self._handlers.append(value) + + self._timer = self.create_timer(self._config["phase"]["control_dt"], self._timer_callback) + + self.load_phase() + + @abstractmethod + def publisher(self, action): + pass + + @abstractmethod + def load_phase(self): + pass + + @abstractmethod + def obs(self): + pass + + +class ConfigError(Exception): + pass diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py new file mode 100644 index 0000000000..69db9b39f3 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py @@ -0,0 +1,55 @@ +import time + +import numpy as np +from handlers.command_handler import CommandHandler +from handlers.gravity_handler import GravityHandler +from handlers.gyro_handler import GyroHandler +from handlers.joint_handler import JointHandler + +from bitbots_msgs.msg import JointCommand +from nodes.rl_node import RLNode + + +class WalkNode(RLNode): + def __init__(self, config_path: str): + # Configuring self._config, self._phase, self._previous_action + super().__init__(config_path, node_name="walk_node") + + # publishers + self._joint_command_pub = self.create_publisher(JointCommand, "walking_motor_goals", 10) + + # handlers + self._gyro_handler = GyroHandler(self._config) + self._gravity_handler = GravityHandler(self._config) + self._joint_handler = JointHandler(self._config) + self._command_handler = CommandHandler(self._config) + + # loading model + model = self._config["model"] + self.load_model(model) + + # observations + def obs(self): + return np.hstack( + [ + self._gyro_handler.get_gyro(), + self._gravity_handler.get_gravity(), + self._command_handler.get_command(), + self._joint_handler.get_velocity_data(), + self._joint_handler.get_angle_data(), + self._previous_action.get_previous_action(), + self._phase.get_obs_phase(), + ] + ).astype(np.float32) + + # load phase function + def load_phase(self): + timestamp = self.get_clock().now().to_msg() + walkready_command = self._joint_handler.get_walkready_joint_command(timestamp=timestamp) + self._joint_command_pub.publish(walkready_command) + time.sleep(10) + + # publisher function + def publisher(self, onnx_pred): + joint_command = self._joint_handler.get_joint_commands(onnx_pred) + self._joint_command_pub.publish(joint_command) diff --git a/src/bitbots_motion/bitbots_rl_motion/package.xml b/src/bitbots_motion/bitbots_rl_motion/package.xml index 38546d848f..787aebb0f8 100644 --- a/src/bitbots_motion/bitbots_rl_motion/package.xml +++ b/src/bitbots_motion/bitbots_rl_motion/package.xml @@ -2,30 +2,24 @@ bitbots_rl_motion - 1.0.0 - The bitbots_rl_motion package provides different reinforcement based motions like walking and standing up. + 0.0.0 + TODO: Package description + florian + TODO: License declaration - Marc Bestmann - Hamburg Bit-Bots - - MIT - - Marc Bestmann - Hamburg Bit-Bots - - ament_cmake - - bitbots_docs - bitbots_robot_description - bitbots_utils - rclpy - std_msgs + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + python3-onnxruntime-pip + std_msgs + bitbots_msgs + sensor_msgs + geometry_msgs + tf2_ros + rclpy - - tested_integration - python3 - - ament_cmake + ament_python diff --git a/src/bitbots_motion/bitbots_rl_motion/resource/bitbots_rl_motion b/src/bitbots_motion/bitbots_rl_motion/resource/bitbots_rl_motion new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/bitbots_motion/bitbots_rl_motion/scripts/rl_walk.py b/src/bitbots_motion/bitbots_rl_motion/scripts/rl_walk.py deleted file mode 100755 index c614c82324..0000000000 --- a/src/bitbots_motion/bitbots_rl_motion/scripts/rl_walk.py +++ /dev/null @@ -1,59 +0,0 @@ -#! /usr/bin/env python3 - -import os -import sys - -import rclpy -import yaml -from ament_index_python import get_package_share_directory -from deep_quintic import env -from deep_quintic.ros_runner import ALGOS, create_test_env, get_saved_hyperparams -from rclpy import Parameter -from rclpy.node import Node - -if __name__ == "__main__": - rclpy.init() - node = Node("rl_walk") - node.declare_parameter("model_folder", Parameter.Type.STRING) - model_folder = node.get_parameter("model_folder").get_parameter_value().string_value - package_path = get_package_share_directory("bitbots_rl_motion") - model_folder = os.path.join(package_path, "rl_walk_models", model_folder) - hyperparams, stats_path = get_saved_hyperparams(model_folder, norm_reward=False, test_mode=True) - - # load env_kwargs if existing - env_kwargs = {} - args_path = os.path.join(model_folder, "args.yml") - if os.path.isfile(args_path): - with open(args_path) as f: - loaded_args = yaml.load(f, Loader=yaml.UnsafeLoader) # pytype: disable=module-attr - if loaded_args["env_kwargs"] is not None: - env_kwargs = loaded_args["env_kwargs"] - else: - node.get_logger().fatal(f"No args.yml found in {args_path}") - sys.exit() - - env_kwargs["node"] = node - print(env_kwargs) - venv = create_test_env( - "ExecuteEnv-v1", - n_envs=1, - stats_path=stats_path, - log_dir=None, - should_render=False, - hyperparams=hyperparams, - env_kwargs=env_kwargs, - ) - - # direct reference to wolfgang env object - env = venv.venv.envs[0].env.env # noqa - - custom_objects = { - "learning_rate": 0.0, - "lr_schedule": lambda _: 0.0, - "clip_range": lambda _: 0.0, - } - model_path = os.path.join(model_folder, "model") - node.get_logger().info(f"Loading model from {model_path}") - model = ALGOS[loaded_args["algo"]].load(model_path, env=venv, custom_objects=custom_objects) - - env.run_node(model, venv) diff --git a/src/bitbots_motion/bitbots_rl_motion/setup.cfg b/src/bitbots_motion/bitbots_rl_motion/setup.cfg new file mode 100644 index 0000000000..3ed963cb53 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/bitbots_rl_motion +[install] +install_scripts=$base/lib/bitbots_rl_motion diff --git a/src/bitbots_motion/bitbots_rl_motion/setup.py b/src/bitbots_motion/bitbots_rl_motion/setup.py index aa8cf91da0..36e1623169 100644 --- a/src/bitbots_motion/bitbots_rl_motion/setup.py +++ b/src/bitbots_motion/bitbots_rl_motion/setup.py @@ -1,11 +1,35 @@ -from distutils.core import setup +import glob -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import find_packages, setup -d = generate_distutils_setup( - packages=["bitbots_rl_motion"], - # scripts=['bin/myscript'], - package_dir={"": "src"}, -) +package_name = "bitbots_rl_motion" -setup(**d) +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ( + "share/" + package_name + "/models", + glob.glob("models/*.onnx"), + ), + ( + "share/" + package_name + "/configs", + glob.glob("configs/*.yaml") + ), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="BitBots", + maintainer_email="git@bit-bots.de", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "run_policies = bitbots_rl_motion.policy_nodes:main", + ], + }, +) diff --git a/src/bitbots_motion/bitbots_rl_motion/test/test_copyright.py b/src/bitbots_motion/bitbots_rl_motion/test/test_copyright.py new file mode 100644 index 0000000000..60c2d1e688 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_copyright.main import main + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/src/bitbots_motion/bitbots_rl_motion/test/test_flake8.py b/src/bitbots_motion/bitbots_rl_motion/test/test_flake8.py new file mode 100644 index 0000000000..4c267ca038 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_flake8.main import main_with_errors + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, f"Found {len(errors)} code style errors / warnings:\n" + "\n".join(errors) diff --git a/src/bitbots_motion/bitbots_rl_motion/test/test_pep257.py b/src/bitbots_motion/bitbots_rl_motion/test/test_pep257.py new file mode 100644 index 0000000000..4eddb46ed1 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_pep257.main import main + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings"