diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index fdad66d77..0c17fb4e3 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/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 diff --git a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index aa72fdfdf..0ffbd6354 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/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 diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd index e93900f33..62bef6e34 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd +++ b/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 diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/joint_handler.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/joint_handler.py index 6daa7d653..23e89d996 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/joint_handler.py +++ b/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 diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/robot_state_handler.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/robot_state_handler.py index 8c0985b97..9e045e8e6 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/robot_state_handler.py +++ b/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. diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/soccer_command_handler.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/soccer_command_handler.py index 7f1f5766b..3a4761db8 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/handlers/soccer_command_handler.py +++ b/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: diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/kick_ball_node.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/kick_ball_node.py index d1f927cfb..01d875bed 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/kick_ball_node.py +++ b/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. diff --git a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/rl_node.py b/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/rl_node.py index e41b9a49b..a20d80158 100644 --- a/src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/nodes/rl_node.py +++ b/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. diff --git a/src/bitbots_motion/bitbots_rl_motion/configs/kick_ball_model.yaml b/src/bitbots_motion/bitbots_rl_motion/configs/kick_ball_model.yaml index 7d6f2611f..2dd8aab97 100644 --- a/src/bitbots_motion/bitbots_rl_motion/configs/kick_ball_model.yaml +++ b/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