Reduce jerk at startup

bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml

@@ -1,7 +1,7 @@
 wolfgang_hardware_interface:
   ros__parameters:
     control_loop_hz: 500.0
-    start_delay: 2.0 # delay after the motor power is turned on until values are written, in seconds
+    start_delay: 2.0 # delay after the motor power is turned on until values are written, in seconds TODO also only read values after this time
 
     port_info:
       port0:

bitbots_misc/bitbots_utils/bitbots_utils/utils.py

@@ -130,6 +130,28 @@ def get_parameters_from_other_node(
     return results
 
 
+def get_parameters_from_other_node_sync(
+    own_node: Node, other_node_name: str, parameter_names: List[str], service_timeout_sec: float = 20.0
+) -> Dict:
+    """
+    Used to receive parameters from other running nodes. It does not use async internally.
+    It should not be used in callback functions, but it it a bit more reliable than the async version.
+    Returns a dict with requested parameter name as dict key and parameter value as dict value.
+    """
+    client = own_node.create_client(GetParameters, f"{other_node_name}/get_parameters")
+    ready = client.wait_for_service(timeout_sec=service_timeout_sec)
+    if not ready:
+        raise RuntimeError(f"Wait for {other_node_name} parameter service timed out")
+    request = GetParameters.Request()
+    request.names = parameter_names
+    response = client.call(request)
+
+    results = {}  # Received parameter
+    for i, param in enumerate(parameter_names):
+        results[param] = parameter_value_to_python(response.values[i])
+    return results
+
+
 def set_parameters_of_other_node(
     own_node: Node,
     other_node_name: str,

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py

@@ -21,15 +21,6 @@ def perform(self, reevaluate=False):
         raise NotImplementedError
 
 
-class TurnMotorsOn(AbstractChangeMotorPower):
-    def perform(self, reevaluate=False):
-        if not self.blackboard.visualization_active and not self.blackboard.simulation_active:
-            req = SetBool.Request()
-            req.data = True
-            self.blackboard.motor_switch_service.call(req)
-        return self.pop()
-
-
 class TurnMotorsOff(AbstractChangeMotorPower):
     def perform(self, reevaluate=False):
         if not self.blackboard.visualization_active and not self.blackboard.simulation_active:

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py

@@ -6,6 +6,7 @@
 
 Just waits for something (i.e. that preconditions will be fullfilled)
 """
+
 from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement
 
 
@@ -34,3 +35,16 @@ def perform(self, reevaluate=False):
         # Pop if the time has elapsed
         if self.blackboard.node.get_clock().now().nanoseconds / 1e9 > self.start_time + self.duration:
             self.pop()
+
+
+class WaitRosControlStartDelay(Wait):
+    """
+    The motors need some time to start up,
+    the time at which we start sending commands is defined in the config of the ros_control node.
+    This action waits for that time.
+    """
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.duration = self.blackboard.motor_start_delay
+        self.blackboard.node.get_logger().info("Waiting for the motors to start up...")

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py

@@ -12,6 +12,7 @@ class CheckMotors(AbstractHCMDecisionElement):
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
         self.had_problem = False
+        self.power_was_off = False
 
     def perform(self, reevaluate=False):
         self.clear_debug_data()
@@ -71,8 +72,16 @@ def perform(self, reevaluate=False):
                     # wait for motors to connect
                     return "PROBLEM"
             else:
-                # we have to turn the motors on
-                return "TURN_ON"
+                # The motors are off, so we will not complain
+                self.power_was_off = True
+                return "MOTORS_NOT_STARTED"
+
+        elif self.power_was_off:
+            # motors are now on and we can continue
+            self.blackboard.node.get_logger().info("Motors are now connected. Will resume.")
+            self.power_was_off = False
+            # But we want to perform a clean start, so we don't jump directly into the last goal position
+            return "TURN_ON"
 
         if self.had_problem:
             # had problem before, just tell that this is solved now

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

@@ -1,17 +1,20 @@
 #EMERGENCY_FALL
 @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait + time:1 + r:false, @TurnMotorsOff, @Wait
 
+#INIT_PATTERN
+@RobotStateStartup, @SetTorque + stiff:false + r:false, @WaitRosControlStartDelay + r:false, @SetTorque + stiff:true + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
+
 -->HCM
 $StartHCM
     START_UP --> $Simulation
         YES --> @RobotStateStartup, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
-        NO --> @RobotStateStartup, @Wait + time:1 + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
+        NO --> #INIT_PATTERN
     RUNNING --> $CheckMotors
         MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait
         OVERLOAD --> #EMERGENCY_FALL
         OVERHEAT --> #EMERGENCY_FALL
         PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
-        TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:walkready + r:false, @Wait
+        TURN_ON --> #INIT_PATTERN
         OKAY --> $RecordAnimation
             RECORD_ACTIVE --> @RobotStateRecord, @Wait
             FREE --> $TeachingMode

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -1,6 +1,7 @@
 from typing import List, Optional
 
 import numpy
+from bitbots_utils.utils import get_parameters_from_other_node_sync
 from geometry_msgs.msg import Twist
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -32,6 +33,9 @@ def __init__(self, node: Node):
         self.visualization_active: bool = self.node.get_parameter("visualization_active").value
         self.pickup_accel_threshold: float = self.node.get_parameter("pick_up_accel_threshold").value
         self.pressure_sensors_installed: bool = self.node.get_parameter("pressure_sensors_installed").value
+        self.motor_start_delay: int = get_parameters_from_other_node_sync(
+            self.node, "/wolfgang_hardware_interface", ["start_delay"]
+        )["start_delay"]
 
         # Create service clients
         self.foot_zero_service = self.node.create_client(EmptySrv, "set_foot_zero")

bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py

@@ -56,7 +56,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
         )
 
         # Create own executor for Python part
-        multi_executor = MultiThreadedExecutor()
+        multi_executor = MultiThreadedExecutor(num_threads=10)
         multi_executor.add_node(self.node)
         self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True)
         self.spin_thread.start()