From 89bdda81dc9c9bc96e848fa8bd686496f9531aa5 Mon Sep 17 00:00:00 2001 From: manx52 Date: Mon, 15 Jul 2024 15:40:15 +0200 Subject: [PATCH] added a equation to convert global coords into local --- .../walk_engine/foot_step_planner.py | 12 ++++-- .../walk_engine/walk_engine.py | 15 ++++++- .../test/test_pybullet_walk.py | 39 +++++++++++-------- .../detector_fieldline.py | 2 +- 4 files changed, 46 insertions(+), 22 deletions(-) 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 index 89aad1b52..6caa0ed12 100644 --- 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 @@ -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 diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine.py index 79e04d28d..0d9c415db 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine.py +++ b/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) diff --git a/soccer_control/soccer_pycontrol/test/test_pybullet_walk.py b/soccer_control/soccer_pycontrol/test/test_pybullet_walk.py index 0c47c6bbc..15c7bd200 100644 --- a/soccer_control/soccer_pycontrol/test/test_pybullet_walk.py +++ b/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 diff --git a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py b/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py index bfecc6465..515dbf94b 100755 --- a/soccer_perception/soccer_object_localization/src/soccer_object_localization/detector_fieldline.py +++ b/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)