From b1c15cc21e3f0de983f78609d8ef8d54837a677c Mon Sep 17 00:00:00 2001 From: Yifei Zhou Date: Sat, 16 Nov 2024 15:57:25 -0500 Subject: [PATCH] clean up pt 3??? --- .../src/soccer_pycontrol/model/sensors.py | 4 ---- .../src/soccer_pycontrol/walk_engine/navigator.py | 11 ++--------- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py index c0beaea9f..e7ef0734b 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py @@ -19,10 +19,6 @@ def __init__(self, body: pb.loadURDF): self.imu_ready = False self.get_imu() # to init - # joint_names = [pb.getJointInfo(self.body, i)[1].decode("utf-8") for i in range(pb.getNumJoints(self.body))] - # print(f"JOINT NAMES") - # print(joint_names) - def get_pose(self, link=None) -> Transformation: """ Get the 3D pose of the robot diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py index 010c0b278..e32ab8fa4 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py @@ -16,9 +16,7 @@ # TODO change to trajectory controller class Navigator: - def __init__( - self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = True - ): # todo: add display option + def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = True): self.world = world self.bez = bez self.imu_feedback_enabled = imu_feedback_enabled @@ -184,8 +182,6 @@ def walk_ball(self, target_goal: Transformation): print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3)) self.foot_step_planner.configure_planner(dx, dy, dtheta) - # todo: get sensor data, etc. and store in matrix - t = self.walk_loop(t) def walk_time(self, target_goal: list): @@ -273,9 +269,7 @@ def filter_joints(self) -> List[int]: return joints def display_walking_metrics(self, show_targets: bool = False) -> None: - metrics = " ".join(self.walking_data.keys()) # todo: remove if statements, just graph everything - - show_targets = True # for testing!!! + metrics = " ".join(self.walking_data.keys()) if "IMU" in metrics: fig, (ax_imu0, ax_imu1, ax_imu2) = plt.subplots(3, 1, sharex=True) @@ -432,7 +426,6 @@ def clear_walking_metrics(self, target_data: list = None) -> None: if name in target_data: self.walking_data[name] = [] - # todo: add function to update stored data def update_walking_metrics(self, t: float) -> None: """update stored data for time t"""