Add sit mode to HCM to help a graceful shutdown

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"

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")

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

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

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -70,13 +70,17 @@ 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
 
         # 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

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

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