diff --git a/pink_balancer/height_controller.py b/pink_balancer/height_controller.py index fa40999..3340c87 100644 --- a/pink_balancer/height_controller.py +++ b/pink_balancer/height_controller.py @@ -17,6 +17,7 @@ from pink.utils import custom_configuration_vector from pink.visualization import start_meshcat_visualizer from upkie.utils.clamp import clamp +from upkie.utils.spdlog import logging from .utils import abs_bounded_derivative_filter @@ -213,6 +214,8 @@ def __init__( visualizer = start_meshcat_visualizer(robot) add_target_frames(visualizer) + logging.info("Initializing Upkie to its neutral configuration...") + self.__initialized = False self.ik_configuration = neutral_configuration self.jump_playback = None @@ -441,6 +444,7 @@ def get_init_servo_action(self, observation: dict, dt: float) -> dict: # Difference is also OK, configuration space is a vector space q_diff = self.q_init - self.ik_configuration.q if np.linalg.norm(q_diff, ord=1) < 1e-5: + logging.info("Upkie initialized to the neutral configuration") self.__initialized = True return_configuration = pink.Configuration( self.robot.model, self.robot.data, self.q_init