creating a framework (not working)

Author: sWintermoor

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -59,7 +59,7 @@ def __init__(self, node, blackboard):
 
         # Ball state
         self._ball: PointStamped = PointStamped(
-            header=Header(stamp=Time(clock_type=ClockType.ROS_TIME).to_msg(), frame_id=self.map_frame)
+            header=Header(stamp=Time(clock_type=ClockType.ROS_TIME), frame_id=self.map_frame)
         )  # The ball in the map frame (default to the center of the field if ball is not seen yet)
         self._ball_covariance: np.ndarray = np.zeros((2, 2))  # Covariance of the ball
 

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/kick_ball.py

@@ -100,7 +100,11 @@ def perform(self, reevaluate=False):
 
                     # only check 2 directions, left and right
                     kick_direction = self.blackboard.costmap.get_best_kick_direction(
-                        -self.penalty_kick_angle, self.penalty_kick_angle, 2, self.kick_length, self.angular_range
+                        -self.penalty_kick_angle,
+                        self.penalget_ball_position_uvty_kick_angle,
+                        2,
+                        self.kick_length,
+                        self.angular_range,
                     )
                 else:
                     ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv()
@@ -141,6 +145,7 @@ def perform(self, reevaluate=False):
         goal_pose.header.stamp = self.blackboard.node.get_clock().now().to_msg()
         goal_pose.header.frame_id = self.blackboard.world_model.base_footprint_frame
 
+        # TODO: Extract
         ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv()
         goal_pose.pose.position.x = ball_u
         goal_pose.pose.position.y = ball_v

src/bitbots_misc/bitbots_bringup/launch/motion.launch

@@ -39,7 +39,7 @@
         <arg name="sim" value="$(var sim)" />
     </include>
 
-    <!-- launch the walking and kicking -->
+    <!-- launch the walking -->
     <group if="$(var walking)">
         <node pkg="bitbots_rl_walk" exec="walk" output="screen">
             <param name="use_sim_time" value="$(var sim)"/>

src/bitbots_rl_walk/bitbots_rl_walk/kick.py

@@ -104,7 +104,7 @@ def __init__(self):
         # Load the ONNX model
         self._onnx_session = rt.InferenceSession(ONNX_MODEL, providers=["CPUExecutionProvider"])
 
-        self._joint_command_pub = self.create_publisher(JointCommand, "DynamixelController/command", 10)
+        self._joint_command_pub = self.create_publisher(JointCommand, "kick_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)
@@ -122,7 +122,7 @@ def __init__(self):
 
         joint_command.positions = WALKREADY_STATE
         self._joint_command_pub.publish(joint_command)
-        time.sleep(20)
+        time.sleep(1)
 
     def _joint_state_callback(self, msg: JointState):
         self._joint_state = msg
@@ -166,6 +166,8 @@ def _timer_callback(self):
                 self.get_logger().warning("Waiting for goal pose data", throttle_duration_sec=1.0)
 
             return
+        else:
+            self.get_logger().warning("RL Kick has all data!")
 
         # TODO consider IMU mounting offset
 

src/bitbots_rl_walk/bitbots_rl_walk/models.py

@@ -0,0 +1,21 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from handler.gyro_handler import GyroHandler
+
+from src.rl_node import RLNode
+
+walk_policy_path = os.path.join(get_package_share_directory("bitbots_rl_walk"), "models", "wolfgang_walk_ppo.onnx")
+kick_policy_path = os.path.join(get_package_share_directory("bitbots_rl_walk"), "models", "wolfgang_kick_ppo.onnx")
+
+# Attention: Handler structure should match obs structure!
+walk_policy_obs_stack = [
+    GyroHandler(),
+    # GravityHandler(),
+]
+
+walk_policy_publishers = []
+
+
+walk_node = RLNode(walk_policy_path)
+kick_node = RLNode(kick_policy_path)

src/bitbots_rl_walk/bitbots_rl_walk/walk.py

@@ -175,7 +175,7 @@ def __init__(self):
         # Load the ONNX model
         self._onnx_session = rt.InferenceSession(ONNX_MODEL, providers=["CPUExecutionProvider"])
 
-        self._joint_command_pub = self.create_publisher(JointCommand, "DynamixelController/command", 10)
+        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)

src/bitbots_rl_walk/bitbots_rl_walk/walk_node.py

@@ -0,0 +1,38 @@
+from geometry_msgs.msg import Twist
+from handler.command_handler import CommandHandler
+from handler.gravity_handler import GravityHandler
+from handler.gyro_handler import GyroHandler
+from handler.joints_handler import JointsHandler
+from sensor_msgs.msg import Imu, JointState
+
+from bitbots_msgs.msg import JointCommand
+from src.rl_node import RLNode
+
+
+class WalkNode(RLNode):
+    def __init__(self, walk_policy_path):
+        super().__init__(walk_policy_path)
+
+        # publishers
+        self._joint_command_pub = self.create_publisher(JointCommand, "walking_motor_goals", 10)
+
+        # suscribers
+        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)
+
+        # handlers
+        self._gyro_handler = GyroHandler()
+        self._gravity_handler = GravityHandler()
+        self._joints_handler = JointsHandler()
+        self._command_handler = CommandHandler()
+
+    def _imu_callback(self, msg):
+        self._gyro_handler.imu_callback(msg)
+        self._gravity_handler.imu_callback(msg)
+
+    def _joint_state_callback(self, msg):
+        self._joints_handler.joint_state_callback(msg)
+
+    def _cmd_vel_callback(self, msg):
+        self._command_handler.cmd_vel_callback(msg)

src/bitbots_rl_walk/handler/command_handler.py

@@ -0,0 +1,13 @@
+import numpy as np
+
+
+class CommandHandler:
+    def __init__(self):
+        self._cmd_vel = None
+
+    def get_data(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 cmd_vel_callback(self, msg):
+        self._cmd_vel = msg

src/bitbots_rl_walk/handler/gravity_handler.py

@@ -0,0 +1,28 @@
+import numpy as np
+from transforms3d.euler import euler2mat
+from transforms3d.quaternions import quat2mat
+
+
+class GravityHandler:
+    def __init__(self):
+        self._imu_data = None
+        self._gravity = None
+
+    # Callables
+    def imu_callback(self, msg):
+        self._imu_data = msg
+
+    def get_data(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

src/bitbots_rl_walk/handler/gyro_handler.py

@@ -0,0 +1,32 @@
+import numpy as np
+from sensor_msgs.msg import Imu
+
+
+# TODO: Adapt to software
+class GyroHandler:
+    def __init__(self, subscribers):
+        self._gyro = {"subscriber": {"Imu": [Imu, "imu/data", self.imu_callback, 10]}, "name": "gyro"}
+
+        # TODO:
+
+    # Callables
+    def imu_callback(self, msg):
+        self._imu_data = msg
+
+    def set(self, key, value):
+        self._gyro[key] = value
+
+    def get_data(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
+
+    def get_confg(self):
+        return self._gyro