Kick warm up observation bufferand tighter kick timings

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -34,11 +34,11 @@ $DoOnce
 $KickOrDribble + map_goal_distance:%ball_approach_dist // should be same as below $GoToBall + target:map action
     KICK --> $DoOnce
         NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick + blocking:false
-        DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:%rl_kick_approach_tolerance_pos + orientation_threshold:%rl_kick_approach_tolerance_rad + latch:true
+        DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:%rl_kick_approach_tolerance_pos + orientation_threshold:%rl_kick_approach_tolerance_deg + latch:true
             NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick
             YES --> $DoOnce
                 NOT_DONE --> @StoreBallMovementDetectionStartPosition
-                DONE --> @ChangeAction + action:kicking, @Stand + duration:0.1 + r:false, @LookAtBallPenalty + r:false, @RLKickTowardsGoal + strength:3.0 + r:false, @ForgetBall
+                DONE --> @ChangeAction + action:kicking + r:false, @LookAtFront + r:false, @RLKickTowardsGoal + strength:3.5 + r:false, @ForgetBall
     DRIBBLE --> $AvoidBall // old behavior before kick when we want to kick the ball in
         NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle
             YES --> $BallKickArea

src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -152,9 +152,9 @@ body_behavior:
     dribble_kick_angle: 0.6
 
     kick_decision_smoothing: 10.0
-
-    rl_kick_approach_tolerance_pos: 0.20
-    rl_kick_approach_tolerance_rad: 0.3
+    
+    rl_kick_approach_tolerance_pos: 0.04
+    rl_kick_approach_tolerance_deg: 10.0
 
     rl_kick:
       timeout: 2.0  # how long the kick policy is active

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

@@ -33,11 +33,11 @@ $StartHCM
                                     NOT_FALLING --> $PlayingExternalAnimation
                                         ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
                                         ANIMATION_SERVER_TIMEOUT --> @CancelAnimation
-                                        FREE --> $RecentWalkingGoals
-                                            STAY_WALKING --> @RobotStateWalking, @Wait
-                                            NOT_WALKING --> $RecentKickGoals
-                                                KICKING --> @RobotStateKicking, @Wait
-                                                NOT_KICKING --> @RobotStateControllable, @Wait
+                                        FREE --> $RecentKickGoals
+                                            KICKING --> @RobotStateKicking, @Wait
+                                            NOT_KICKING -->$RecentWalkingGoals
+                                                STAY_WALKING --> @RobotStateWalking, @Wait
+                                                NOT_WALKING --> @RobotStateControllable, @Wait
                                     ELSE --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFalling, @Wait
                                 ELSE --> $GameControllerStop
                                     STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/joint_handler.py

@@ -12,7 +12,7 @@ def __init__(self, node):
         self._node = 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._walkready_state = np.array(self._node.get_parameter("joints.walkready_state").value, dtype=np.float32)
         self._action_scales = np.array(self._node.get_parameter("joints.action_scales").value, dtype=np.float32)
         self._joint_signs = np.array(self._node.get_parameter("joints.joint_signs").value, dtype=np.float32)
         self._kp = np.array(self._node.get_parameter("joints.kp").value, dtype=np.float32)
@@ -32,6 +32,16 @@ def __init__(self, node):
         self._joint_state_sub = self._node.create_subscription(
             JointState, "joint_states", self._joint_state_callback, 10
         )
+
+        # Latest joint command actually being executed by the robot (combined
+        # motor goals published by the HCM). Used to reconstruct what action the
+        # policy would have produced while it is not running, so the action
+        # history can be kept warm. Keyed by joint name; a message may carry only
+        # a subset of joints (e.g. head-only), so entries persist across updates.
+        self._latest_command: dict[str, float] = {}
+        self._joint_command_sub = self._node.create_subscription(
+            JointCommand, "joint_command", self._joint_command_callback, 10
+        )
         self._joint_command = JointCommand()
         self._joint_command.joint_names = published_joint_names
 
@@ -48,6 +58,10 @@ def _joint_state_callback(self, msg: JointState):
             self._joint_state_indices = None
         self._joint_state = msg
 
+    def _joint_command_callback(self, msg: JointCommand):
+        for name, position in zip(msg.joint_names, msg.positions):
+            self._latest_command[name] = position
+
     def has_data(self):
         return self._joint_state is not None
 
@@ -113,6 +127,30 @@ def get_joint_commands(self, onnx_pred, relative_to_current: bool = False) -> Jo
         self._joint_command.positions = positions[self._publish_indices]
         return self._joint_command
 
+    def reconstruct_previous_action(self) -> np.ndarray:
+        """Infer the action that would have produced the currently executed joint command.
+
+        While the policy is not running, its action-history term has no real
+        previous action to feed back. This reconstructs a plausible one by
+        inverting the default-pose-relative mapping used in ``get_joint_commands``
+        (``positions = (action * action_scales + walkready_state) * joint_signs``)
+        applied to the joint command the robot is actually executing:
+
+            ``action = (positions * joint_signs - walkready_state) / action_scales``
+
+        (``joint_signs`` is +-1, so multiplying by it inverts it.) Joints for
+        which no command has been observed yet fall back to the action that
+        reproduces the measured joint angle, i.e. "hold the current pose".
+        """
+        # Fallback per joint: action reproducing the measured joint angle.
+        # get_angle_data() already returns measured * joint_signs - walkready_state.
+        action = self.get_angle_data() / self._action_scales
+        for i, name in enumerate(self._ordered_relevant_joint_names):
+            position = self._latest_command.get(name)
+            if position is not None:
+                action[i] = (position * self._joint_signs[i] - self._walkready_state[i]) / self._action_scales[i]
+        return action.astype(np.float32)
+
     def get_previous_action_initial(self) -> np.ndarray:
         return self._previous_action
 

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/robot_state_handler.py

@@ -11,6 +11,7 @@
 KICKABLE_STATES = (
     RobotControlState.CONTROLLABLE,
     RobotControlState.KICKING,
+    RobotControlState.WALKING,
 )
 
 # States during which the getup (standup) policy should produce motor goals.

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/soccer_command_handler.py

@@ -83,7 +83,6 @@ def __init__(self, node):
         self._ball_pose: Optional[PoseWithCovarianceStamped] = None
 
         # kick state: set by the action (its own thread), read by the control loop
-        self._wait_for_controllable = False
         self._warm_start_active = False
         self._kick_active = False
         self._kick_abort_requested = False
@@ -227,19 +226,6 @@ def _execute_kick(self, goal_handle):
         self._last_kick_dir_b = self._kick_dir_body
         self._kick_abort_requested = False
 
-        self._wait_for_controllable = True
-        while self._wait_for_controllable:
-            if goal_handle.is_cancel_requested:
-                goal_handle.canceled()
-                result = Kick.Result()
-                result.result = Kick.Result.ABORTED
-                return result
-            if self._node._robot_state_handler.is_kickable():
-                self._wait_for_controllable = False
-            else:
-                self._node.get_logger().warn("Waiting for robot to be controllable before starting kick...")
-                self._node.get_clock().sleep_for(Duration(seconds=0.02))
-
         # Warm start: run the policy with a defined command so it can settle into a
         # in domain pose and the observation history fills
         if self._warm_start_duration > 0.0:

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/kick_ball_node.py

@@ -86,18 +86,30 @@ def publisher(self, onnx_pred):
     def allowed_states(self):
         return self._robot_state_handler.is_kickable() and self._soccer_command_handler.is_kick_active()
 
+    def passive_update(self):
+        # Keep the observation histories warm while the kick is NOT running, so
+        # that when it activates the policy already sees a saturated history
+        # instead of a cold one -- without paying for network inference.
+        #
+        # Every observation term is filled from the real sensors as usual. The
+        # one term that has no real value while the policy is idle is the
+        # previous action; we mock it by reconstructing the action that would
+        # have produced the joint command the robot is currently executing
+        # (published by another controller via the HCM). Feeding that into the
+        # previous-action term lets the shared obs() build the action history
+        # identically to the active path, and also gives the first active step a
+        # continuous previous action to feed back.
+        self._previous_action.set_previous_action(self._joint_handler.reconstruct_previous_action())
+        # Advance all histories (and the soccer command/ball histories) by one
+        # step. The returned observation is intentionally discarded: no inference.
+        self.obs()
+
     def initialize_observation(self):
-        # Clear all 8-frame observation histories and the soccer command/ball
-        # histories so the first observation after activation re-saturates them
-        # with fresh sensor data instead of stale pre-activation frames.
-        self._ang_vel_hist.reset()
-        self._gravity_hist.reset()
-        self._joint_pos_hist.reset()
-        self._joint_vel_hist.reset()
-        self._action_hist.reset()
-        self._soccer_command_handler.reset()
-        # Start the previous-action feedback term from zero on (re)activation.
-        self._previous_action.set_previous_action(np.zeros_like(self._previous_action.get_previous_action()))
+        # Nothing to reset: the observation histories are kept continuously warm
+        # by passive_update() while the kick is inactive, so on activation they
+        # already hold fresh sensor data (and a reconstructed previous action)
+        # rather than stale or zeroed frames.
+        pass
 
     def _phase_update_hook(self):
         # This policy does not use a gait phase.

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/rl_node.py

@@ -72,11 +72,13 @@ def _timer_callback(self):
             )
             return
 
-        # Only run the policy while it is in an allowed (active) state. When it is
-        # not active nothing is observed, inferred or published; the next
-        # activation starts from a clean observation state.
+        # Only run the policy (inference + publish) while it is in an allowed
+        # (active) state. When it is not active the expensive network is skipped,
+        # but nodes may still keep their observation history warm via
+        # passive_update() so a later activation starts from a saturated history.
         if not self.allowed_states():
             self._policy_active = False
+            self.passive_update()
             return
 
         # First step of a (re)activation: let the node initialize its observation
@@ -133,6 +135,18 @@ def _timer_callback(self):
     def _phase_update_hook(self) -> None:
         pass
 
+    def passive_update(self) -> None:
+        """Per-step update while the policy is inactive. Default: no-op.
+
+        Called once per control tick (after the sensor-ready check) whenever the
+        node is not in an allowed state, i.e. when no inference/publish happens.
+        Nodes that need their observation history buffers to stay warm while
+        inactive override this to advance those histories -- without running the
+        network -- so activation starts from a saturated history instead of a
+        cold one.
+        """
+        pass
+
     @abstractmethod
     def initialize_observation(self):
         """Reset/seed the observation state at the start of each activation.

src/bitbots_motion/bitbots_rl_motion/configs/kick_ball_model.yaml

@@ -115,26 +115,26 @@ kick_ball_node:
       # Per-joint sign mapping between the robot's joint convention and the
       # policy's (HighTorque/IsaacLab training URDF) convention, applied to
       # joint pos/vel on read and to the joint target on write.
-      
+
       joint_signs: [
-        1.0,    # head_yaw          
+        1.0,    # head_yaw
         1.0,    # l_hip_pitch
         1.0,    # l_shoulder_pitch
         -1.0,   # r_hip_pitch
         -1.0,   # r_shoulder_pitch
-        1.0,    # head_pitch        
+        1.0,    # head_pitch
         1.0,    # l_hip_roll
-        -1.0,    # l_shoulder_roll   
+        -1.0,    # l_shoulder_roll
         1.0,    # r_hip_roll
-        -1.0,    # r_shoulder_roll   
+        -1.0,    # r_shoulder_roll
         -1.0,   # l_thigh
-        -1.0,    # l_upper_arm     
+        -1.0,    # l_upper_arm
         -1.0,   # r_thigh
-        -1.0,    # r_upper_arm      
+        -1.0,    # r_upper_arm
         -1.0,   # l_calf
-        -1.0,    # l_elbow          
+        -1.0,    # l_elbow
         1.0,    # r_calf
-        1.0,    # r_elbow          
+        1.0,    # r_elbow
         1.0,    # l_ankle_pitch
         -1.0,   # r_ankle_pitch
         1.0,    # l_ankle_roll
@@ -180,8 +180,8 @@ kick_ball_node:
     #                      ball history on the 50 Hz loop). Must match training.
     #   history_samples  : stacked command/ball history length. Must match training.
     command:
-      kick_timeout: 2.0
-      warm_start_duration: 1.0
+      kick_timeout: 1.5
+      warm_start_duration: 0.0
       warm_start_command: [0.0, 0.0, 0.0]
       post_kick_command_duration: 1.0
       post_stabilization_stop_duration: 1.0