diff --git a/kos_sim/__init__.py b/kos_sim/__init__.py index f102a9c..48a9f97 100644 --- a/kos_sim/__init__.py +++ b/kos_sim/__init__.py @@ -1 +1,13 @@ +import logging + +# Configure root logger +logging.basicConfig( + level=logging.DEBUG, + format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", + handlers=[logging.StreamHandler()], +) + +# Get logger for this package +logger = logging.getLogger("kos_sim") + __version__ = "0.0.1" diff --git a/kos_sim/config.py b/kos_sim/config.py index fb79d1b..500bf7e 100644 --- a/kos_sim/config.py +++ b/kos_sim/config.py @@ -14,8 +14,13 @@ class SimulatorConfig: joint_name_to_id: dict[str, int] kp: float = 80.0 kd: float = 10.0 - physics_freq: float = 1000.0 # Hz - command_freq: float = 50.0 # Hz + dt: float = 0.02 + command_freq: float = 50.0 # Hz + + @property + def physics_freq(self) -> float: + """Calculate physics frequency from timestep.""" + return 1.0 / self.dt @property def sim_decimation(self) -> int: @@ -37,7 +42,7 @@ def from_file(cls, config_path: str) -> "SimulatorConfig": joint_name_to_id=joint_name_to_id, kp=control_config.get("kp", 80.0), kd=control_config.get("kd", 10.0), - physics_freq=control_config.get("physics_freq", 1000.0), + dt=control_config.get("dt", 0.001), command_freq=control_config.get("command_freq", 50.0), ) diff --git a/kos_sim/server.py b/kos_sim/server.py index fb62006..7123bae 100644 --- a/kos_sim/server.py +++ b/kos_sim/server.py @@ -1,7 +1,6 @@ """Server and simulation loop for KOS.""" import argparse -import logging import threading import time from concurrent import futures @@ -9,21 +8,14 @@ import grpc from kos_protos import actuator_pb2_grpc, imu_pb2_grpc +from kos_sim import logger +from kos_sim.config import SimulatorConfig from kos_sim.services import ActuatorService, IMUService from kos_sim.simulator import MujocoSimulator -from kos_sim.config import SimulatorConfig - -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger(__name__) class SimulationServer: - def __init__( - self, - model_path: str, - config_path: str | None = None, - port: int = 50051 - ) -> None: + def __init__(self, model_path: str, config_path: str | None = None, port: int = 50051) -> None: if config_path: config = SimulatorConfig.from_file(config_path) else: @@ -61,13 +53,13 @@ def start(self) -> None: self._grpc_thread = threading.Thread(target=self._grpc_server_loop) self._grpc_thread.start() - physics_dt = 1.0 / self.simulator._config.physics_freq try: while not self._stop_event.is_set(): process_start = time.time() self.simulator.step() - # Sleep to maintain physics frequency - time.sleep(max(0, physics_dt - (time.time() - process_start))) + sleep_time = max(0, self.simulator._config.dt - (time.time() - process_start)) + logger.debug("Sleeping for %f seconds", sleep_time) + time.sleep(sleep_time) except KeyboardInterrupt: self.stop() diff --git a/kos_sim/services.py b/kos_sim/services.py index 5bb6afc..e48fe9c 100644 --- a/kos_sim/services.py +++ b/kos_sim/services.py @@ -40,9 +40,7 @@ def GetActuatorsState( # noqa: N802 return actuator_pb2.GetActuatorsStateResponse( states=[ actuator_pb2.ActuatorStateResponse( - actuator_id=joint_id, - position=math.degrees(float(state)), - online=True + actuator_id=joint_id, position=math.degrees(float(state)), online=True ) for joint_id, state in states.items() ] diff --git a/kos_sim/simulator.py b/kos_sim/simulator.py index f4c6985..682bf7e 100644 --- a/kos_sim/simulator.py +++ b/kos_sim/simulator.py @@ -1,17 +1,14 @@ """Wrapper around MuJoCo simulation.""" -import logging import threading import mujoco import mujoco_viewer import numpy as np +from kos_sim import logger from kos_sim.config import SimulatorConfig -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger(__name__) - class MujocoSimulator: def __init__( @@ -19,7 +16,6 @@ def __init__( model_path: str, config: SimulatorConfig | None = None, render: bool = False, - dt: float = 0.002, gravity: bool = True, suspended: bool = False, ) -> None: @@ -28,7 +24,7 @@ def __init__( # Load MuJoCo model and initialize data self._model = mujoco.MjModel.from_xml_path(model_path) - self._model.opt.timestep = dt + self._model.opt.timestep = self._config.dt # Use dt from config self._data = mujoco.MjData(self._model) self._gravity = gravity @@ -114,8 +110,8 @@ def step(self) -> None: for i in range(self._model.njnt): if self._model.jnt_type[i] == mujoco.mjtJoint.mjJNT_FREE: print(f"Joint name: {self._model.joint(i).name}") - self._data.qpos[i:i + 7] = self._model.keyframe("default").qpos[i:i + 7] - self._data.qvel[i:i + 6] = 0 + self._data.qpos[i : i + 7] = self._model.keyframe("default").qpos[i : i + 7] + self._data.qvel[i : i + 6] = 0 break if self._render_enabled: diff --git a/kos_sim/test_client.py b/kos_sim/test_client.py index 167d6cd..26a6c07 100644 --- a/kos_sim/test_client.py +++ b/kos_sim/test_client.py @@ -1,14 +1,12 @@ """Test script for the simulation server.""" import argparse -import logging import math import time import pykos -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger(__name__) +from kos_sim import logger def test_actuator_commands(host: str = "localhost", port: int = 50051) -> None: diff --git a/kos_sim/test_simulator.py b/kos_sim/test_simulator.py index 81db64c..ac94ab1 100644 --- a/kos_sim/test_simulator.py +++ b/kos_sim/test_simulator.py @@ -1,14 +1,10 @@ """Test script for the simulator.""" import argparse -import logging import time from kos_sim.simulator import MujocoSimulator -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger(__name__) - def test_simulation(model_path: str, duration: float = 5.0, speed: float = 1.0, render: bool = True) -> None: simulator = MujocoSimulator(model_path, render=render)