added a equation to convert global coords into local

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py

@@ -43,15 +43,17 @@ def __init__(
 
         self.current_step_time = 0
 
-    def create_path_to_goal(self, start_pose: Transformation, end_pose: Transformation) -> PathRobot:
+    def create_path_to_goal(self, end_pose: Transformation) -> PathRobot:
         """
         Creates a path from the robot's current location to the goal location
 
         :param end_pose: 3D transformation
         :return: Robot path
         """
+        # TODO make all end goals relative
         # TODO is this the best place for it?
-        start_pose.position = [start_pose.position[0], start_pose.position[1], self.walking_torso_height]
+        # start_pose.position = [start_pose.position[0], start_pose.position[1], self.walking_torso_height]
+
         end_pose.position = [end_pose.position[0], end_pose.position[1], self.walking_torso_height]
 
         # Remove the roll and pitch from the designated position
@@ -71,7 +73,11 @@ def create_path_to_goal(self, start_pose: Transformation, end_pose: Transformati
         # 2]:.3f} {end_pose_calibrated.quaternion[3]:.3f}]\033[0m" )
 
         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
+            Transformation([0, 0, self.walking_torso_height], [0, 0, 0, 1]),
+            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

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine.py

@@ -1,4 +1,6 @@
+import numpy as np
 import pybullet as pb
+import scipy
 from soccer_pycontrol.common.links import Links
 from soccer_pycontrol.model.bez import Bez
 from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld
@@ -42,13 +44,22 @@ def __init__(self, world: PybulletWorld, bez: Bez):
         # TODO should this be an input?
         self.t = 0
 
-    def set_goal(self, goal: Transformation) -> None:
+    def set_goal(self, goal: Transformation, transform_global: bool = True) -> None:
         """
         Set the goal of the robot, will create the path to the goal that will be executed in the run() loop
 
         :param goal: The 3D location goal for the robot
         """
-        self.step_planner.create_path_to_goal(self.bez.sensors.get_pose(), goal)
+        if transform_global:
+            goal = self.transform_global_local(goal)
+
+        self.step_planner.create_path_to_goal(goal)
+
+    def transform_global_local(self, goal: Transformation) -> Transformation:
+        current_pose = self.bez.sensors.get_pose()
+        goal.rotation_matrix = np.matmul(goal.rotation_matrix, scipy.linalg.inv(current_pose.rotation_matrix))
+        goal.position = current_pose.rotation_matrix.T @ goal.position - current_pose.rotation_matrix.T @ current_pose.position
+        return goal
 
     def wait(self, step: int) -> None:
         self.world.wait(step)

soccer_control/soccer_pycontrol/test/test_pybullet_walk.py

@@ -3,6 +3,7 @@
 import numpy as np
 import pybullet as pb
 import pytest
+import scipy
 from soccer_pycontrol.common.links import Links
 from soccer_pycontrol.model.bez import Bez
 from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld
@@ -42,7 +43,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, cameraTargetPosition=[0, 0, 0.45])
-        bez = Bez(robot_model="bez1")
+        bez = Bez(robot_model="bez2")
         tf = WalkEngine(world, bez)
         tf.wait(50)
         tf.ready()
@@ -54,36 +55,41 @@ def test_foot_step_planner_plane(self):
         tf.wait(100)
 
     def test_walk_2(self):
+        start = Transformation([-0.7384, -0.008, 0], [0, 0, 0, 1])
+        goal = Transformation([0.0198, -0.0199, 0], [0, 0, 0, 1])
         self.custom_walk(
             cameraTargetPosition=[0, 0, 0.25],
-            start_pose=Transformation([-0.7384, -0.008, 0], [0, 0, 0, 1]),
-            goal_pose=Transformation([0.0198, -0.0199, 0], [0, 0, 0, 1]),
+            start_pose=start,
+            goal_pose=goal,
         )
 
     def test_walk_3(self):
+        start = Transformation([-2.404, -1.0135, 0], [0, 0, -0.9979391070307153, 0.064168050139])
+        goal = Transformation([-2.26, -1.27, 0], [0, 0, 0.997836202477347, 0.06574886330262358])
         self.custom_walk(
-            cameraTargetPosition=[-2, -1, 0.25],
-            start_pose=Transformation([-2.404, -1.0135, 0], [0, 0, -0.9979391070307153, 0.064168050139]),
-            goal_pose=Transformation([-2.26, -1.27, 0], [0, 0, 0.997836202477347, 0.06574886330262358]),
+            cameraTargetPosition=[0, 0, 0.25],
+            start_pose=start,
+            goal_pose=goal,
         )
 
     def test_walk_4(self):
+        # start = Transformation([0.3275415, 0.2841, 0.321], euler=(1.57,0.0, 0))
+        # goal = Transformation([-0.12015226, -0.19813691, 0.321], [0, 0, 0.95993011, -0.28023953])
+        start = Transformation([0.3275415, 0.2841, 0.321], [0.04060593, 0.0120126, 0.86708929, -0.4963497])
+        goal = Transformation([-0.12015226, -0.19813691, 0.321], [0, 0, 0.95993011, -0.28023953])
         self.custom_walk(
             cameraTargetPosition=[0, 0, 0.25],
-            start_pose=Transformation([0.3275415, 0.2841, 0.321], [0.04060593, 0.0120126, 0.86708929, -0.4963497]),
-            goal_pose=Transformation([-0.12015226, -0.19813691, 0.321], [0, 0, 0.95993011, -0.28023953]),
+            start_pose=start,
+            goal_pose=goal,
         )
-        # self.custom_walk(
-        #     cameraTargetPosition=[0, 0, 0.25],
-        #     start_pose=Transformation([0,0,0], euler=(1.57, 0, 0)),
-        #     goal_pose=Transformation([1, 0, 0], euler=(-1.57, 0, 0)),
-        # )
 
     def test_walk_5(self):
+        start = Transformation([0.716, -0.4188, 0.0], [0.0149, -0.085, 0.9685, 0.2483])
+        goal = Transformation([0.0859, -0.016, 0.0], [0, 0, 0.998, 0.0176])
         self.custom_walk(
             cameraTargetPosition=[0, 0, 0.25],
-            start_pose=Transformation([0.716, -0.4188, 0.0], [0.0149, -0.085, 0.9685, 0.2483]),
-            goal_pose=Transformation([0.0859, -0.016, 0.0], [0, 0, 0.998, 0.0176]),
+            start_pose=start,
+            goal_pose=goal,
         )
 
     def test_walk_side(self):
@@ -106,6 +112,7 @@ def test_turn_in_place(self):
             cameraTargetPosition=[0, 0, 0.45], start_pose=Transformation([0, 0, 0], [0.00000, 0, 0, 1]), goal_pose=Transformation(euler=[np.pi, 0, 0])
         )
 
+    # TODO fix later for new relative goal
     def test_small_movement_0(self):
         self.custom_walk(
             cameraTargetPosition=[0, 0, 0.45],
@@ -192,7 +199,7 @@ def test_foot_step_planner_plane_dynamic(self):
         tf.ready()
         tf.wait(50)
         for i in range(10):
-            tf.set_goal(Transformation([0.2, 0, 0], [0, 0, 0, 1]))
+            tf.set_goal(Transformation([0.2, 0, 0], [0, 0, 0, 1]), transform_global=False)
             tf.walk()
         tf.wait(100)
         # TODO WIP need a way to figure out how to add more steps so it doesnt stop

soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py

@@ -75,7 +75,7 @@ def image_callback(self, img: Image, debug=False):
             return
         image_crop = image[h + 1 :, :, :]
         # image_crop_blurred = cv2.GaussianBlur(image_crop, (3, 3), 0)
-        image_crop_blurred = cv2.bilateralFilter(image_crop, 9, 75, 75)
+        image_crop_blurred = cv2.bilateralFilter(image, 9, 75, 75)
 
         hsv = cv2.cvtColor(src=image_crop_blurred, code=cv2.COLOR_BGR2HSV)