From 32268fff1ac4275a6ceecf03a412102fc349b0b5 Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Sun, 6 Sep 2026 13:32:58 +0200 Subject: [PATCH 1/2] Add sit mode to HCM to help a graceful shutdown --- .../hcm_dsd/actions/play_animation.py | 6 +++ .../hcm_dsd/decisions/animation.py | 12 +++++ .../bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd | 52 ++++++++++--------- .../bitbots_hcm/hcm_dsd/hcm_blackboard.py | 4 ++ .../bitbots_hcm/humanoid_control_module.py | 9 +++- .../bitbots_hcm/config/hcm_piplus.yaml | 1 + 6 files changed, 58 insertions(+), 26 deletions(-) diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py index 2002e35c3b..f9223c02dd 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py +++ b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py @@ -98,6 +98,12 @@ def animation_finished(self): ) or self.blackboard.animation_action_current_goal.cancelled() +class PlayAnimationSit(AbstractPlayAnimation): + def choose_animation(self): + self.blackboard.node.get_logger().info("PLAYING SIT ANIMATION") + return self.blackboard.animation_name_sit + + class PlayAnimationFallingLeft(AbstractPlayAnimation): def choose_animation(self): self.blackboard.node.get_logger().info("PLAYING FALLING LEFT ANIMATION") diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py index 99d94e2342..5069745143 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py +++ b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py @@ -57,3 +57,15 @@ def perform(self, reevaluate=False): def get_reevaluate(self): return True + + +class ForceSitting(AbstractHCMDecisionElement): + def perform(self, reevaluate=True): + self.publish_debug_data("Sitting", self.blackboard.sitting) + if self.blackboard.sitting: + return "SITTING" + else: + return "NORMAL" + + def get_reevaluate(self): + return True 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 0414c4611d..690c789217 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 @@ -15,28 +15,30 @@ $StartHCM OVERHEAT --> #EMERGENCY_FALL PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors TURN_ON --> #INIT_PATTERN - OKAY --> $RecordAnimation - RECORD_ACTIVE --> @RobotStateRecord, @Wait - FREE --> $TeachingMode - TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait - HOLD --> @SetTorque + stiff:true, @Wait - FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationWalkReady - OFF --> $Stop - STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationWalkReady, @Wait - FREE -->$CheckIMU - IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup - PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU - OKAY --> $Fallen - NOT_FALLEN --> $Falling - 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 - ELSE --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFalling, @Wait - ELSE --> $GameControllerStop - STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait - FREE --> @RobotStateFallen + r:false, @CancelGoals + r:false, @StopWalking + r:false, @RobotStateGettingUp + r:false, @Complain + r:false, @Wait + time:1.5 + r:false + OKAY --> $ForceSitting + SITTING --> @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationSit + r:false, @SetTorque + stiff:false, @Wait + NORMAL --> $RecordAnimation + RECORD_ACTIVE --> @RobotStateRecord, @Wait + FREE --> $TeachingMode + TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait + HOLD --> @SetTorque + stiff:true, @Wait + FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationWalkReady + OFF --> $Stop + STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationWalkReady, @Wait + FREE -->$CheckIMU + IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup + PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU + OKAY --> $Fallen + NOT_FALLEN --> $Falling + 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 + ELSE --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFalling, @Wait + ELSE --> $GameControllerStop + STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait + FREE --> @RobotStateFallen + r:false, @CancelGoals + r:false, @StopWalking + r:false, @RobotStateGettingUp + r:false, @Complain + r:false, @Wait + time:1.5 + r:false diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py index 61af7f7eb3..d87fd04609 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py +++ b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py @@ -70,6 +70,7 @@ def __init__(self, node: Node): self.animation_name_stand_up_back: str = self.node.get_parameter("animations.stand_up_back").value self.animation_name_stand_up_front: str = self.node.get_parameter("animations.stand_up_front").value self.animation_name_startup: str = self.node.get_parameter("animations.startup").value + self.animation_name_sit: str = self.node.get_parameter("animations.sit").value self.animation_name_walk_ready: str = self.node.get_parameter("animations.walk_ready").value self.animation_name_turning_front_left: str = self.node.get_parameter("animations.turning_front_left").value self.animation_name_turning_front_right: str = self.node.get_parameter("animations.turning_front_right").value @@ -77,6 +78,9 @@ def __init__(self, node: Node): # Teaching State self.teaching_mode_state: int = SetTeachingMode.Request.OFF + # Sitting + self.sitting = False + # Motor State self.current_joint_state: Optional[JointState] = None self.previous_joint_state: Optional[JointState] = None diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py index 592932cbe1..c5ae6d3547 100755 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py +++ b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py @@ -20,7 +20,7 @@ from ros2_numpy import numpify from sensor_msgs.msg import Imu, JointState from std_msgs.msg import Bool, Float32 -from std_srvs.srv import SetBool +from std_srvs.srv import Empty, SetBool from bitbots_hcm import hcm_dsd from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard @@ -91,6 +91,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active): self.manual_penalize_service = self.node.create_service( ManualPenalize, "manual_penalize", self.set_manual_penalize_mode_callback ) + self.sit_service = self.node.create_service(Empty, "sit", self.sit_callback) # Store time of the last tick self.last_tick_start_time = self.node.get_clock().now() @@ -165,6 +166,12 @@ def get_state(self) -> T_RobotControlState: """Returns the current state of the HCM.""" return self.blackboard.current_state + def sit_callback(self, req: Empty.Request, resp: Empty.Response): + """Sits the robot down and turns of the motors. (non-reversable)""" + self.blackboard.sitting = True + self.node.get_logger().warn(f"Received sit request. {self.blackboard.sitting=}") + return resp + def set_animation_mode_callback(self, request: SetBool.Request, response: SetBool.Response): # Check if the robot is in a state where it is allowed to play animations if request.data: # We want to go into the animation mode diff --git a/src/bitbots_motion/bitbots_hcm/config/hcm_piplus.yaml b/src/bitbots_motion/bitbots_hcm/config/hcm_piplus.yaml index e4dc1fb09f..8ec4a9a74f 100644 --- a/src/bitbots_motion/bitbots_hcm/config/hcm_piplus.yaml +++ b/src/bitbots_motion/bitbots_hcm/config/hcm_piplus.yaml @@ -26,6 +26,7 @@ turning_front_left: "turning_front_left" turning_front_right: "turning_front_right" walk_ready: "walkready" + sit: "sit" # Falling stand_up_active: true # Enables the robot to stand up automatically From e261fba826bf81b4658666685ec2661b3f2e06e2 Mon Sep 17 00:00:00 2001 From: Cornelius Krupp Date: Sun, 6 Sep 2026 13:43:51 +0200 Subject: [PATCH 2/2] Add sit task --- pixi.toml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pixi.toml b/pixi.toml index aa19965ac0..2b8cbe63ea 100644 --- a/pixi.toml +++ b/pixi.toml @@ -39,6 +39,10 @@ description = "Runs tests for all ROS 2 packages. Pass additional colcon args (e # Skipping packages in the src/lib directory as they are not "our" responsibility. They should be tested separately in their own repositories. cmd = "colcon test --parallel-workers 1 --event-handlers console_direct+ --return-code-on-test-failure --packages-skip audio_common domain_bridge humanoid_base_footprint ipm_image_node soccer_ipm zed_wrapper zed_components zed_msgs zed_ros2 " +[tasks.sit] +description = "Sits the robot down." +cmd = "ros2 service call /sit std_srvs/srv/Empty" + [dependencies] # Base dependencies python = "==3.12.1, <3.13"