diff --git a/src/bitbots_behavior/bitbots_body_behavior/launch/behavior_standalone.launch b/src/bitbots_behavior/bitbots_body_behavior/launch/behavior_standalone.launch index 0f50b104a9..f8691935d1 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/launch/behavior_standalone.launch +++ b/src/bitbots_behavior/bitbots_body_behavior/launch/behavior_standalone.launch @@ -3,7 +3,7 @@ - + diff --git a/src/bitbots_misc/bitbots_bringup/launch/motion.launch b/src/bitbots_misc/bitbots_bringup/launch/motion.launch index b32d55461e..d40fc1587f 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/motion.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/motion.launch @@ -2,9 +2,8 @@ - - + @@ -32,14 +31,10 @@ - - - - - - - - + + + + diff --git a/src/bitbots_misc/bitbots_bringup/launch/motion_standalone.launch b/src/bitbots_misc/bitbots_bringup/launch/motion_standalone.launch index be075ad9a1..afb2328342 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/motion_standalone.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/motion_standalone.launch @@ -3,7 +3,7 @@ - + diff --git a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py b/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py index db8de2586c..a6a6de6641 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py +++ b/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py @@ -90,7 +90,7 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path: def generate_world_xml(num_robots: int, package_share: str, robot_type: str) -> Path: """Generate MuJoCo world XML with the correct number of robots.""" - template_path = Path(package_share) / "xml" / "adult_field.xml" + template_path = Path(package_share) / "xml" / "kid_field.xml" output_path = Path(package_share) / "xml" / "generated_world.xml" offset = 6 * ( 1 / num_robots diff --git a/src/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch b/src/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch index 6d571e3d1e..5b0b03258d 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch @@ -11,6 +11,7 @@ + @@ -29,5 +30,6 @@ + diff --git a/src/bitbots_misc/bitbots_bringup/launch/teamplayer.launch b/src/bitbots_misc/bitbots_bringup/launch/teamplayer.launch index 6d5ee039a2..7ec164da80 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/teamplayer.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/teamplayer.launch @@ -16,8 +16,8 @@ - - + + @@ -28,7 +28,7 @@ - + @@ -42,8 +42,7 @@ - - + diff --git a/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch b/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch index 5dd8e9aede..cf15154173 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch @@ -5,7 +5,7 @@ - + diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml new file mode 100644 index 0000000000..c19a3345d1 --- /dev/null +++ b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml @@ -0,0 +1,19 @@ +################################### +# RoboCup 2026 Standard HSL Field # +################################### + +# Settings for the standard platform league field + +parameter_blackboard: + ros__parameters: + field: + size: + x: 9.0 + y: 6.0 + padding: 1.0 # Padding (area outside of the lines) of the field in all directions + markings: + penalty_area: + size: + x: 2.0 # The distance from the goal line to the boundary of the penalty area + goal: + width: 1.85 # The width of the goal (center of one goal post to the other goal post) diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml new file mode 100644 index 0000000000..1106bab8f2 --- /dev/null +++ b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml @@ -0,0 +1,29 @@ +# Map Generator Config +# This file was generated by the map generator GUI + +header: + version: '1.0' + type: map_generator_config +parameters: + map_type: line + penalty_mark: true + center_point: true + goal_back: true + stroke_width: 5 + field_length: 900 + field_width: 600 + goal_depth: 75 + goal_width: 185 + goal_area_length: 100 + goal_area_width: 300 + penalty_mark_distance: 150 + center_circle_diameter: 150 + border_strip_width: 100 + penalty_area_length: 200 + penalty_area_width: 400 + field_feature_size: 10 + mark_type: cross + field_feature_style: exact + distance_map: false + distance_decay: 0.0 + invert: true diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_black_on_white.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_black_on_white.png new file mode 100644 index 0000000000..e8696a2534 Binary files /dev/null and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_black_on_white.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_white_on_black.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_white_on_black.png new file mode 100644 index 0000000000..fcb8995218 Binary files /dev/null and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid_white_on_black.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png new file mode 100644 index 0000000000..c216972040 Binary files /dev/null and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/launch/parameter_blackboard.launch b/src/bitbots_misc/bitbots_parameter_blackboard/launch/parameter_blackboard.launch index 78d4728d40..ec2e250975 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/launch/parameter_blackboard.launch +++ b/src/bitbots_misc/bitbots_parameter_blackboard/launch/parameter_blackboard.launch @@ -1,8 +1,8 @@ - - + + 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 index 6b57b36405..d7e5f6f7d4 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py +++ b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py @@ -1,26 +1,15 @@ -from typing import Optional - import numpy as np +from rclpy.node import Node class Phase: - _phase: np.ndarray = np.array([0.0, np.pi], dtype=np.float32) - _phase_dt: Optional[float] - - def __init__(self, node): + def __init__(self, node: Node): self._node = node - - if self._node.get_parameter("phase.use_phase").value: - self._control_dt = self._node.get_parameter("phase.control_dt").value - self._gait_frequency = self._node.get_parameter("phase.gait_frequency").value - self._phase_dt = 2 * np.pi * self._gait_frequency * self._control_dt - self._use_phase = True - else: - self._control_dt = None - self._gait_frequency = None - self._phase_dt = None - self._use_phase = False - self._node.get_logger().warning("No phase was found! Using policy without phase!") + self._use_phase = self._node.get_parameter("phase.use_phase").value + self._phase = np.array(self._node.get_parameter("phase.initial_phase").value) + self._control_dt = self._node.get_parameter("phase.control_dt").value + self._gait_frequency = self._node.get_parameter("phase.gait_frequency").value + self._phase_dt = 2 * np.pi * self._gait_frequency * self._control_dt self._obs_phase = None diff --git a/src/bitbots_motion/bitbots_rl_motion/configs/mjlab_walk_model.yaml b/src/bitbots_motion/bitbots_rl_motion/configs/mjlab_walk_model.yaml new file mode 100644 index 0000000000..4cf6ae3872 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/configs/mjlab_walk_model.yaml @@ -0,0 +1,87 @@ +mjlab_walk_node: + ros__parameters: + model: walk_mjlab.onnx + + joints: + ordered_relevant_joint_names: [ + "r_hip_pitch_joint", + "l_hip_pitch_joint", + "r_hip_roll_joint", + "l_hip_roll_joint", + "r_thigh_joint", + "l_thigh_joint", + "r_calf_joint", + "l_calf_joint", + "r_ankle_pitch_joint", + "l_ankle_pitch_joint", + "r_ankle_roll_joint", + "l_ankle_roll_joint", + "r_shoulder_roll_joint", + "l_shoulder_roll_joint", + "r_shoulder_pitch_joint", + "l_shoulder_pitch_joint", + "r_upper_arm_joint", + "l_upper_arm_joint", + "r_elbow_joint", + "l_elbow_joint", + ] + walkready_state: [ + 0.25, -0.25, + 0.0, 0.0, + 0.0, 0.0, + 0.65, -0.65, + 0.4, -0.4, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + ] + kp: [ + # legs + 55.0, 55.0, + 55.0, 55.0, + 55.0, 55.0, + 55.0, 55.0, + 55.0, 55.0, + 55.0, 55.0, + # arms + 6.0, 6.0, + 6.0, 6.0, + 6.0, 6.0, + 6.0, 6.0, + ] + kd: [ + # legs + 1.1, 1.1, + 1.4, 1.4, + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1, + # arms + 0.6, 0.6, + 0.6, 0.6, + 0.6, 0.6, + 0.6, 0.6, + ] + action_scales: [ + # legs + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + # arms + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5 + ] + phase: + control_dt: 0.02 + gait_frequency: 99.99 + use_phase: False + + providers: ["CPUExecutionProvider"] diff --git a/src/bitbots_motion/bitbots_rl_motion/configs/playground_walk_model.yaml b/src/bitbots_motion/bitbots_rl_motion/configs/playground_walk_model.yaml new file mode 100644 index 0000000000..bde31eadc6 --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/configs/playground_walk_model.yaml @@ -0,0 +1,57 @@ +walk_node: + ros__parameters: + model: policy_walk.onnx + + joints: + ordered_relevant_joint_names: [ + "r_hip_pitch_joint", + "r_hip_roll_joint", + "r_thigh_joint", + "r_calf_joint", + "r_ankle_pitch_joint", + "r_ankle_roll_joint", + "l_hip_pitch_joint", + "l_hip_roll_joint", + "l_thigh_joint", + "l_calf_joint", + "l_ankle_pitch_joint", + "l_ankle_roll_joint" + ] + walkready_state: [ + 0.6, 0.0, 0.0, 1.2, 0.6, 0.0, + -0.6, 0.0, 0.0, -1.2, -0.6, 0.0 + ] + kp: [ + # legs + 30.0, 30.0, + 30.0, 30.0, + 30.0, 30.0, + 30.0, 30.0, + 30.0, 30.0, + 30.0, 30.0 + ] + kd: [ + # legs + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1, + 1.1, 1.1 + ] + action_scales: [ + # legs + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5, + 0.5, 0.5 + ] + phase: + use_phase: True + control_dt: 0.02 + gait_frequency: 0.75 + initial_phase: [1.5707963267948966, -1.5707963267948966] + + providers: ["CPUExecutionProvider"] diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py index f206413943..40b3e311e7 100644 --- a/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/command_handler.py @@ -9,15 +9,27 @@ class CommandHandler(Handler): def __init__(self, node): self._node = node - self._cmd_vel: Optional[Twist] = None - self._cmd_vel_sub = self._node.create_subscription(Twist, "cmd_vel", self._cmd_vel_callback, 10) def get_command(self): assert self._cmd_vel is not None - command = np.array([self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z], dtype=np.float32) - return command + return np.array( + [self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z], + dtype=np.float32, + ) + + def get_command_with_stop(self): + assert self._cmd_vel is not None + stop_signal = float(self.get_stop_signal()) + return np.array( + [self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z, stop_signal], + dtype=np.float32, + ) + + def get_stop_signal(self) -> bool: + assert self._cmd_vel is not None + return self._cmd_vel.angular.x != 0.0 def has_data(self): return self._cmd_vel is not None diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py index d0e2b6ba52..ea2041d1b8 100644 --- a/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/gravity_handler.py @@ -2,7 +2,6 @@ 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 @@ -17,23 +16,17 @@ def __init__(self, node): self._imu_sub = self._node.create_subscription(Imu, "imu/data", self._imu_callback, 10) # Callables - def _imu_callback(self, msg): + def _imu_callback(self, msg: Imu): self._imu_data = msg - def has_data(self): + def has_data(self) -> bool: return self._imu_data is not None - def get_gravity(self): - assert self._imu_data is not None - 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) + def get_gravity(self) -> np.ndarray: + """ + Returns the gravity vector in the robot's base frame computed from the IMU orientation. + """ + assert self.has_data(), "IMU data is not available yet" + q = self._imu_data.orientation + gravity = quat2mat([q.w, q.x, q.y, q.z]).T @ np.array([0, 0, -1], dtype=np.float32) return gravity diff --git a/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py b/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py index a1871fd598..02de138f7c 100644 --- a/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py +++ b/src/bitbots_motion/bitbots_rl_motion/handlers/joint_handler.py @@ -13,12 +13,25 @@ def __init__(self, node): self._ordered_relevant_joint_names = self._node.get_parameter("joints.ordered_relevant_joint_names").value self._walkready_state = self._node.get_parameter("joints.walkready_state").value + self._action_scales = np.array(self._node.get_parameter("joints.action_scales").value, dtype=np.float32) + self._kp = np.array(self._node.get_parameter("joints.kp").value, dtype=np.float32) + self._kd = np.array(self._node.get_parameter("joints.kd").value, dtype=np.float32) self._previous_action: np.ndarray = np.zeros(len(self._ordered_relevant_joint_names), dtype=np.float32) self._joint_state: Optional[JointState] = None self._joint_state_sub = self._node.create_subscription( JointState, "joint_states", self._joint_state_callback, 10 ) + self._joint_command = JointCommand() + self._joint_command.joint_names = self._ordered_relevant_joint_names + + self._joint_command.velocities = [-1.0] * len(self._ordered_relevant_joint_names) + self._joint_command.accelerations = [-1.0] * len(self._ordered_relevant_joint_names) + # self._joint_command.max_torques = [-1.0] * len(self._ordered_relevant_joint_names) + self._joint_command.kp = self._kp + self._joint_command.kd = self._kd + + self._joint_state_indices = None def _joint_state_callback(self, msg): self._joint_state = msg @@ -26,14 +39,13 @@ def _joint_state_callback(self, msg): def has_data(self): return self._joint_state is not None - def get_angle_data(self): + def get_angle_data(self) -> np.ndarray: assert self._joint_state is not None + if self._joint_state_indices is None: + self._cache_joint_state_indices() joint_angles = ( np.array( - [ - self._joint_state.position[self._joint_state.name.index(name)] - for name in self._ordered_relevant_joint_names - ], + [self._joint_state.position[idx] for idx in self._joint_state_indices], dtype=np.float32, ) - self._walkready_state @@ -41,29 +53,23 @@ def get_angle_data(self): return joint_angles - def get_velocity_data(self): + def get_velocity_data(self) -> np.ndarray: assert self._joint_state is not None + if self._joint_state_indices is None: + self._cache_joint_state_indices() joint_velocities = np.array( - [ - self._joint_state.velocity[self._joint_state.name.index(name)] - for name in self._ordered_relevant_joint_names - ], + [self._joint_state.velocity[idx] for idx in self._joint_state_indices], dtype=np.float32, ) - return joint_velocities - def get_joint_commands(self, onnx_pred): - assert self._joint_state is not None - 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_joint_commands(self, onnx_pred) -> JointCommand: + self._joint_command.header.stamp = self._joint_state.header.stamp # self._node.get_clock().now().to_msg() + self._joint_command.positions = onnx_pred * self._action_scales + self._walkready_state + return self._joint_command - def get_previous_action_initial(self): + def get_previous_action_initial(self) -> np.ndarray: return self._previous_action + + def _cache_joint_state_indices(self): + self._joint_state_indices = [self._joint_state.name.index(name) for name in self._ordered_relevant_joint_names] diff --git a/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch b/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch index 8436dce764..c0f9e8f3a1 100644 --- a/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch +++ b/src/bitbots_motion/bitbots_rl_motion/launch/rl_motion.launch @@ -1,11 +1,13 @@ - + + + - + @@ -14,4 +16,13 @@ + + + + + + + + + diff --git a/src/bitbots_motion/bitbots_rl_walk/models/policy_walk.onnx b/src/bitbots_motion/bitbots_rl_motion/models/policy_walk.onnx similarity index 100% rename from src/bitbots_motion/bitbots_rl_walk/models/policy_walk.onnx rename to src/bitbots_motion/bitbots_rl_motion/models/policy_walk.onnx diff --git a/src/bitbots_motion/bitbots_rl_motion/models/walk_mjlab.onnx b/src/bitbots_motion/bitbots_rl_motion/models/walk_mjlab.onnx new file mode 100644 index 0000000000..45dc0d1a66 Binary files /dev/null and b/src/bitbots_motion/bitbots_rl_motion/models/walk_mjlab.onnx differ diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/mjlab_walk_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/mjlab_walk_node.py new file mode 100644 index 0000000000..bc9ce7079e --- /dev/null +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/mjlab_walk_node.py @@ -0,0 +1,59 @@ +import numpy as np +from handlers.ball_handler import BallHandler +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 handlers.robot_state_handler import RobotStateHandler + +from bitbots_msgs.msg import JointCommand +from nodes.rl_node import RLNode, create_main + + +class MjLabWalkNode(RLNode): + def __init__(self): + # Configuring self._phase, self._previous_action + super().__init__(node_name="mjlab_walk_node") + + # publishers + self._joint_command_pub = self.create_publisher(JointCommand, "walking_motor_goals", 10) + + # handlers + self._gyro_handler = GyroHandler(self) + self._gravity_handler = GravityHandler(self) + self._joint_handler = JointHandler(self) + self._ball_handler = BallHandler(self) + self._robot_state_handler = RobotStateHandler(self) + self._command_handler = CommandHandler(self) + + # loading model + model = self.get_parameter("model").value + 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_angle_data(), + self._joint_handler.get_velocity_data(), + self._previous_action.get_previous_action(), + self._command_handler.get_command(), + ] + ).astype(np.float32) + + return observation + + # publisher function + def publisher(self, onnx_pred): + joint_command = self._joint_handler.get_joint_commands(onnx_pred) + self._joint_command_pub.publish(joint_command) + + # states in which the policy executes + def allowed_states(self): + allowed_to_move = self._robot_state_handler.is_walkable() and np.any(self._command_handler.get_command() != 0.0) + return allowed_to_move + + +main = create_main(MjLabWalkNode) diff --git a/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/phase_from_transform.py b/src/bitbots_motion/bitbots_rl_motion/nodes/phase_from_transform.py similarity index 100% rename from src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/phase_from_transform.py rename to src/bitbots_motion/bitbots_rl_motion/nodes/phase_from_transform.py diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py index 1568d1b356..d289578495 100644 --- a/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py @@ -3,7 +3,6 @@ from pathlib import Path import numpy as np -import onnx import onnxruntime as rt import rclpy from ament_index_python import get_package_share_directory @@ -25,9 +24,19 @@ def __init__(self, node_name: str): self.declare_parameter("phase.control_dt", 0.0) self.declare_parameter("phase.gait_frequency", 0.0) self.declare_parameter("phase.use_phase", False) + self.declare_parameter("phase.initial_phase", [0.0, np.pi]) self.declare_parameter("providers", ["CPUExecutionProvider"]) self.declare_parameter("joints.ordered_relevant_joint_names", [""]) self.declare_parameter("joints.walkready_state", [0.0]) + self.declare_parameter( + "joints.action_scales", [0.5] * len(self.get_parameter("joints.ordered_relevant_joint_names").value) + ) + self.declare_parameter( + "joints.kp", [55.0] * len(self.get_parameter("joints.ordered_relevant_joint_names").value) + ) + self.declare_parameter( + "joints.kd", [0.6] * len(self.get_parameter("joints.ordered_relevant_joint_names").value) + ) model = self.get_parameter("model").value self.get_logger().info(f"Loaded model: {model}") @@ -43,7 +52,7 @@ def _timer_callback(self): 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, + throttle_duration_sec=10.0, ) return @@ -66,10 +75,11 @@ def _timer_callback(self): if self.allowed_states(): self.publisher(onnx_pred) + self._phase_update_hook() - if self._phase.check_phase_set(): - 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) + @abstractmethod + def _phase_update_hook(self): + pass def _all_sensors_ready(self): for handler in self._handlers: @@ -82,13 +92,12 @@ 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) - + self.get_logger().warning(f"Loading ONNX model from {self._onnx_model_path}") # Load the ONNX model self._onnx_session = rt.InferenceSession(self._onnx_model_path, providers=self.get_parameter("providers").value) - 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._onnx_input_name = [inp.name for inp in self._onnx_session.get_inputs()] + self._onnx_output_name = [out.name for out in self._onnx_session.get_outputs()] self._handlers = [] diff --git a/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py b/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py index f28f46bd21..f5ec6643ee 100644 --- a/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py +++ b/src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py @@ -34,7 +34,7 @@ def obs(self): [ self._gyro_handler.get_gyro(), self._gravity_handler.get_gravity(), - self._command_handler.get_command(), + self._command_handler.get_command_with_stop(), self._joint_handler.get_angle_data(), self._joint_handler.get_velocity_data(), self._previous_action.get_previous_action(), @@ -49,8 +49,23 @@ def publisher(self, onnx_pred): # states in which the policy executes def allowed_states(self): - allowed_to_move = self._robot_state_handler.is_walkable() and np.any(self._command_handler.get_command() != 0.0) - return allowed_to_move + return self._robot_state_handler.is_walkable() + + def _phase_update_hook(self): + if not self._phase.check_phase_set(): + return + phase = self._phase.get_phase() + if self._command_handler.get_stop_signal(): + anchors = [ + np.array([-np.pi / 2, np.pi / 2], dtype=np.float32), + np.array([np.pi / 2, -np.pi / 2], dtype=np.float32), + ] + nearest = min(anchors, key=lambda a: np.linalg.norm(phase - a)) + if np.linalg.norm(phase - nearest) < 0.1: + self._phase.set_phase(nearest) + return + phase_tp1 = phase + self._phase.get_phase_dt() + self._phase.set_phase(np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi) main = create_main(WalkNode) diff --git a/src/bitbots_motion/bitbots_rl_motion/setup.py b/src/bitbots_motion/bitbots_rl_motion/setup.py index 106dc782c3..fabd8a2138 100644 --- a/src/bitbots_motion/bitbots_rl_motion/setup.py +++ b/src/bitbots_motion/bitbots_rl_motion/setup.py @@ -29,6 +29,8 @@ "console_scripts": [ "walk_node = nodes.walk_node:main", "kick_node = nodes.kick_node:main", + "mjlab_walk_node = nodes.mjlab_walk_node:main", + "phase_from_transform = nodes.phase_from_transform:main", ], }, ) diff --git a/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/__init__.py b/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/walk.py b/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/walk.py deleted file mode 100644 index eeb6ef7ba6..0000000000 --- a/src/bitbots_motion/bitbots_rl_walk/bitbots_rl_walk/walk.py +++ /dev/null @@ -1,200 +0,0 @@ -# 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 typing import Optional - -import numpy as np -import onnxruntime as rt -from ament_index_python import get_package_share_directory -from geometry_msgs.msg import Twist -from rclpy.node import Node -from sensor_msgs.msg import Imu, JointState -from transforms3d.euler import euler2mat -from transforms3d.quaternions import quat2mat - -from bitbots_msgs.msg import JointCommand - -ONNX_MODEL = os.path.join(get_package_share_directory("bitbots_rl_walk"), "models", "policy_walk.onnx") - -WALKREADY_STATE = np.array([0.6, 0.0, 0.0, 1.2, 0.6, 0, -0.6, 0.0, 0.0, -1.2, -0.6, 0], dtype=np.float32) - -CONTROL_DT = 0.02 # Control loop frequency in seconds - -GAIT_FREQUENCY = 1.5 # Gait frequency in Hz - -ORDERED_RELEVANT_JOINT_NAMES = [ - "r_hip_pitch_joint", - "r_hip_roll_joint", - "r_thigh_joint", - "r_calf_joint", - "r_ankle_pitch_joint", - "r_ankle_roll_joint", - "l_hip_pitch_joint", - "l_hip_roll_joint", - "l_thigh_joint", - "l_calf_joint", - "l_ankle_pitch_joint", - "l_ankle_roll_joint", -] - - -class WalkNode(Node): - """Node to control the wolfgang humanoid.""" - - _previous_action: np.ndarray = np.zeros(len(ORDERED_RELEVANT_JOINT_NAMES), dtype=np.float32) - _imu_data: Optional[Imu] = None - _joint_state: Optional[JointState] = None - _cmd_vel: Optional[Twist] = None - _phase: np.ndarray = np.array([np.pi / 2, -np.pi / 2], dtype=np.float32) - _phase_dt: float - - def __init__(self): - super().__init__("reinforcement_learning_walk_inference_node") - - # Set sim time parameter to true - # self.set_parameters([ - # Parameter('use_sim_time', Parameter.Type.BOOL, True), ]) - - self._phase_dt = 2 * np.pi * GAIT_FREQUENCY * CONTROL_DT - - # Load the ONNX model - self._onnx_session = rt.InferenceSession(ONNX_MODEL, providers=["CPUExecutionProvider"]) - - self._joint_command_pub = self.create_publisher(JointCommand, "walking_motor_goals", 10) - self._imu_sub = self.create_subscription(Imu, "imu/data", self._imu_callback, 10) - self._joint_state_sub = self.create_subscription(JointState, "joint_states", self._joint_state_callback, 10) - self._cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self._cmd_vel_callback, 10) - - self._timer = self.create_timer(CONTROL_DT, self._timer_callback) - - def _joint_state_callback(self, msg: JointState): - self._joint_state = msg - - def _cmd_vel_callback(self, msg: Twist): - self._cmd_vel = msg - - def _imu_callback(self, msg: Imu): - self._imu_data = msg - - def _timer_callback(self): - """Timer callback to publish joint commands based on the ONNX policy.""" - if self._imu_data is None or self._joint_state is None or self._cmd_vel is None: - self.get_logger().warning("Waiting for all sensors to be available", throttle_duration_sec=1.0) - - # Print the sensor that we are still waiting for - if self._imu_data is None: - self.get_logger().warning("Waiting for IMU data", throttle_duration_sec=1.0) - if self._joint_state is None: - self.get_logger().warning("Waiting for joint state data", throttle_duration_sec=1.0) - if self._cmd_vel is None: - self.get_logger().warning("Waiting for cmd_vel data", throttle_duration_sec=1.0) - - return - - # Prepare the observation vector - gyro = np.array( - [ - self._imu_data.angular_velocity.x, - self._imu_data.angular_velocity.y, - self._imu_data.angular_velocity.z, - ], - dtype=np.float32, - ) - - 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) - - joint_angles = ( - np.array( - [ - self._joint_state.position[self._joint_state.name.index(name)] - for name in ORDERED_RELEVANT_JOINT_NAMES - ], - dtype=np.float32, - ) - - WALKREADY_STATE - ) - - joint_velocities = np.array( - [self._joint_state.velocity[self._joint_state.name.index(name)] for name in ORDERED_RELEVANT_JOINT_NAMES], - dtype=np.float32, - ) - - phase = np.array([np.cos(self._phase), np.sin(self._phase)], dtype=np.float32).flatten() - - stop_signal = self._cmd_vel.angular.x != 0.0 - - command = np.array( - [self._cmd_vel.linear.x, self._cmd_vel.linear.y, self._cmd_vel.angular.z, float(stop_signal)], - dtype=np.float32, - ) - - obs = np.hstack( - [ - gyro, - gravity, - command, - joint_angles, - joint_velocities, - self._previous_action, # Previous action - phase, - ] - ).astype(np.float32) - - # Run the ONNX model - onnx_input = {"obs": obs.reshape(1, -1)} - onnx_pred = self._onnx_session.run(["continuous_actions"], onnx_input)[0][0] - self._previous_action = onnx_pred - - # Publish the joint commands - joint_command = JointCommand() - joint_command.header.stamp = self._joint_state.header.stamp - joint_command.joint_names = ORDERED_RELEVANT_JOINT_NAMES - joint_command.positions = onnx_pred * 0.5 + WALKREADY_STATE - joint_command.velocities = [-1.0] * len(ORDERED_RELEVANT_JOINT_NAMES) - joint_command.accelerations = [-1.0] * len(ORDERED_RELEVANT_JOINT_NAMES) - joint_command.kp = [30.0] * len(ORDERED_RELEVANT_JOINT_NAMES) - joint_command.kd = [1.1] * len(ORDERED_RELEVANT_JOINT_NAMES) - - self._joint_command_pub.publish(joint_command) - - if stop_signal and np.linalg.norm(self._phase - np.array([-np.pi / 2, np.pi / 2])) < 0.1: - self._phase = np.array([-np.pi / 2, np.pi / 2]) - elif stop_signal and np.linalg.norm(self._phase - np.array([np.pi / 2, -np.pi / 2])) < 0.1: - self._phase = np.array([np.pi / 2, -np.pi / 2]) - else: - phase_tp1 = self._phase + self._phase_dt - self._phase = np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi - - -def main(): - import rclpy - - rclpy.init() - node = WalkNode() - rclpy.spin(node) - node.destroy_node() - rclpy.try_shutdown() diff --git a/src/bitbots_motion/bitbots_rl_walk/package.xml b/src/bitbots_motion/bitbots_rl_walk/package.xml deleted file mode 100644 index 6a3b14d9c0..0000000000 --- a/src/bitbots_motion/bitbots_rl_walk/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - bitbots_rl_walk - 0.0.0 - TODO: Package description - florian - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - python3-onnxruntime-pip - std_msgs - bitbots_msgs - sensor_msgs - geometry_msgs - tf2_ros - rclpy - - - ament_python - - diff --git a/src/bitbots_motion/bitbots_rl_walk/resource/bitbots_rl_walk b/src/bitbots_motion/bitbots_rl_walk/resource/bitbots_rl_walk deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/bitbots_motion/bitbots_rl_walk/setup.cfg b/src/bitbots_motion/bitbots_rl_walk/setup.cfg deleted file mode 100644 index 6fc8f9a307..0000000000 --- a/src/bitbots_motion/bitbots_rl_walk/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/bitbots_rl_walk -[install] -install_scripts=$base/lib/bitbots_rl_walk diff --git a/src/bitbots_motion/bitbots_rl_walk/setup.py b/src/bitbots_motion/bitbots_rl_walk/setup.py deleted file mode 100644 index e113e2211b..0000000000 --- a/src/bitbots_motion/bitbots_rl_walk/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -import glob - -from setuptools import find_packages, setup - -package_name = "bitbots_rl_walk" - -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")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="florian", - maintainer_email="git@flova.de", - description="TODO: Package description", - license="TODO: License declaration", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "walk = bitbots_rl_walk.walk:main", - "phase_from_transform = bitbots_rl_walk.phase_from_transform:main", - ], - }, -) diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py index 62eacb7b91..443bb3f80d 100644 --- a/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py +++ b/src/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/simulation.py @@ -60,7 +60,7 @@ def __init__(self): ] def _generate_default_world(self) -> str: - template_path = Path(self.package_path) / "xml" / "adult_field.xml" + template_path = Path(self.package_path) / "xml" / "kid_field.xml" output_path = Path(self.package_path) / "xml" / "generated_world.xml" with open(template_path) as f: template = f.read() @@ -82,7 +82,15 @@ def _find_robot_indices(self) -> list[int]: def run(self) -> None: print("Starting simulation viewer...") - with viewer.launch_passive(self.model, self.data) as view: + with viewer.launch_passive(self.model, self.data, show_left_ui=False, show_right_ui=False) as view: + # pos="7.709 -7.854 6.511" xyaxes="0.726 0.687 -0.000 -0.357 0.377 0.855 + view.cam.type = mujoco.mjtCamera.mjCAMERA_FREE + view.cam.lookat[0] = 0.0 + view.cam.lookat[1] = 0.0 + view.cam.lookat[2] = 0.0 + view.cam.distance = 12.0 + view.cam.azimuth = 135.0 + view.cam.elevation = -30.0 while view.is_running(): start_time = time.perf_counter() self.step() diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml b/src/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml index 3d2be54b10..caaed6941c 100644 --- a/src/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml +++ b/src/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml @@ -6,7 +6,7 @@ - + diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/assets/kid_field.png b/src/bitbots_simulation/bitbots_mujoco_sim/xml/assets/kid_field.png new file mode 100644 index 0000000000..a8c2ecd497 Binary files /dev/null and b/src/bitbots_simulation/bitbots_mujoco_sim/xml/assets/kid_field.png differ diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_adult.xml b/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_adult.xml new file mode 100644 index 0000000000..c6f6f2349e --- /dev/null +++ b/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_adult.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_kid.xml b/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_kid.xml new file mode 100644 index 0000000000..2223544f21 --- /dev/null +++ b/src/bitbots_simulation/bitbots_mujoco_sim/xml/goal_kid.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/kid_field.xml b/src/bitbots_simulation/bitbots_mujoco_sim/xml/kid_field.xml new file mode 100644 index 0000000000..c08fee9a93 --- /dev/null +++ b/src/bitbots_simulation/bitbots_mujoco_sim/xml/kid_field.xml @@ -0,0 +1,36 @@ + + +