fixed the yaml pathways should be good to do

utra-robosoccer · Jul 13, 2024 · 22e73df · 22e73df
1 parent 517c4ef
commit 22e73df
Show file tree

Hide file tree

Showing 5 changed files with 12 additions and 10 deletions.
diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_foot.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_foot.py
@@ -35,7 +35,7 @@ class PathFoot(Path):
     Adds the path of the 2 feet on the left and right of the Path
     """
 
-    def __init__(self, start_transform, end_transform, sim: str = "_sim", robot_model: str = "bez1"):
+    def __init__(self, start_transform, end_transform, foot_center_to_floor: float, sim: str = "_sim", robot_model: str = "bez1"):
         super().__init__(start_transform, end_transform, sim=sim, robot_model=robot_model)
 
         # : A half step is taken on the first and the last step to get the robot moving, this parameter indicates the
@@ -65,7 +65,7 @@ def __init__(self, start_transform, end_transform, sim: str = "_sim", robot_mode
         #: Whether the first step is the left foot
         self.first_step_left = False
 
-        self.foot_center_to_floor = self.parameters["foot_center_to_floor"]
+        self.foot_center_to_floor = foot_center_to_floor
 
     @functools.lru_cache
     def half_step_time(self):

diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_robot.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_robot.py
@@ -17,12 +17,13 @@ class PathRobot(PathTorso):
 
     def __init__(
         self,
-        start_transform: Transformation = Transformation(),
-        end_transform: Transformation = Transformation([0.5, 0, 0], [0, 0, 0, 1]),
+        start_transform: Transformation,
+        end_transform: Transformation,
+        foot_center_to_floor: float,
         sim: str = "_sim",
         robot_model: str = "bez1",
     ):
-        super().__init__(start_transform, end_transform, sim=sim, robot_model=robot_model)
+        super().__init__(start_transform, end_transform, foot_center_to_floor=foot_center_to_floor, sim=sim, robot_model=robot_model)
 
     def show(self):
         fig = plt.figure(tight_layout=True, figsize=(10, 10))

diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_torso.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/path/path_torso.py
@@ -4,16 +4,15 @@
 import numpy as np
 import rospy
 import scipy
-from pre_commit import yaml
 from soccer_pycontrol.exp.calibration import adjust_navigation_transform
 from soccer_pycontrol.path.path_foot import PathFoot
 
 from soccer_common.transformation import Transformation
 
 
 class PathTorso(PathFoot):
-    def __init__(self, start_transform, end_transform, sim: str = "_sim", robot_model: str = "bez1"):
-        super().__init__(start_transform, end_transform, sim=sim, robot_model=robot_model)
+    def __init__(self, start_transform, end_transform, foot_center_to_floor, sim: str = "_sim", robot_model: str = "bez1"):
+        super().__init__(start_transform, end_transform, foot_center_to_floor=foot_center_to_floor, sim=sim, robot_model=robot_model)
 
         #: How much the torso bounces up and down while following the torso trajectory (m)
         self.torso_zdiff_sway = self.parameters["torso_zdiff_sway"]

diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py
@@ -70,7 +70,9 @@ def create_path_to_goal(self, start_pose: Transformation, end_pose: Transformati
         # end_pose_calibrated.quaternion[0]:.3f} {end_pose_calibrated.quaternion[1]:.3f} {end_pose_calibrated.quaternion[
         # 2]:.3f} {end_pose_calibrated.quaternion[3]:.3f}]\033[0m" )
 
-        # self.robot_path = PathRobot(start_pose, end_pose_calibrated, sim=self.sim, robot_model=self.robot_model,foot_center_to_floor )
+        self.robot_path = PathRobot(
+            start_pose, end_pose_calibrated, foot_center_to_floor=self.foot_center_to_floor, sim=self.sim, robot_model=self.robot_model
+        )
 
         # TODO this edits the rate for the controller need to figure out how to use it
         self.current_step_time = 0

diff --git a/soccer_control/soccer_pycontrol/test/test_pybullet.py b/soccer_control/soccer_pycontrol/test/test_pybullet.py
@@ -85,7 +85,7 @@ def test_foot_step_planner_fixed(self):
 
     def test_foot_step_planner_plane(self):
         world = PybulletWorld(camera_yaw=90, real_time=True, rate=100)
-        bez = Bez(robot_model="bez1")  # , pose=Transformation(euler=[0,0,-1.57/2.0])) bez1  bez2
+        bez = Bez(robot_model="bez2")  # , pose=Transformation(euler=[0,0,-1.57/2.0])) bez1  bez2
         tf = WalkEngine(world, bez)
         tf.wait(50)
         tf.ready()