Skip to content

Commit

Permalink
Log messages about initialization phase
Browse files Browse the repository at this point in the history
  • Loading branch information
stephane-caron committed Oct 14, 2024
1 parent 4fe355f commit 153eee7
Showing 1 changed file with 4 additions and 0 deletions.
4 changes: 4 additions & 0 deletions pink_balancer/height_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 153eee7

Please sign in to comment.