From 5c998423d535c3db6381fb334c4e533f5cfeb98b Mon Sep 17 00:00:00 2001 From: manx52 Date: Wed, 4 Dec 2024 17:53:41 -0500 Subject: [PATCH 01/24] added states to bez. fixed pybullet camera and tried to align with webots and reallife but needs more tuning. added a soccerball to pybullet world. added camera link to assembly placo urdf. minor edit to dae script. made the calculate_ball_from_bounding_boxes arg better arranged. disable cover horizon for now. added a new set of test that run object detection on pybullet camera images. Added both methods for object localization. starting to fill out strategy. --- .idea/workspace.xml | 158 +++++++++++------- .../src/soccer_pycontrol/model/bez.py | 41 ++++- .../src/soccer_pycontrol/model/sensors.py | 25 +-- .../pybullet_usage/pybullet_world.py | 3 + .../walk_engine/foot_step_planner.py | 6 +- .../soccer_pycontrol/walk_engine/navigator.py | 7 +- .../soccer_pycontrol/test/test_placo.py | 3 +- .../soccer_pycontrol/test/test_pybullet.py | 2 + .../assembly_description/urdf/robot.urdf | 16 +- .../scripts/convert_stl2dae.py | 4 +- .../camera/camera_calculations.py | 2 +- .../object_detect_node.py | 4 +- .../object_detect_node_ros.py | 2 +- .../test/test_camera.py | 4 +- .../test/test_pybullet_camera.py | 115 +++++++++++++ .../src/soccer_strategy/behavior/behavior.py | 20 +++ .../behavior/behavior_context.py | 14 +- .../soccer_strategy/behavior/state/balance.py | 4 +- .../soccer_strategy/behavior/state/fallen.py | 14 ++ .../behavior/state/find_ball.py | 31 ++++ .../soccer_strategy/behavior/state/walk.py | 14 ++ .../src/soccer_strategy/behavior_executive.py | 64 +++---- .../soccer_strategy/behavior_executive_ros.py | 74 ++++++++ 23 files changed, 495 insertions(+), 132 deletions(-) create mode 100644 soccer_perception/soccer_object_detection/test/test_pybullet_camera.py create mode 100644 soccer_strategy/src/soccer_strategy/behavior/state/fallen.py create mode 100644 soccer_strategy/src/soccer_strategy/behavior/state/find_ball.py create mode 100644 soccer_strategy/src/soccer_strategy/behavior/state/walk.py create mode 100644 soccer_strategy/src/soccer_strategy/behavior_executive_ros.py diff --git a/.idea/workspace.xml b/.idea/workspace.xml index cb46f465d..75dc21c37 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -5,29 +5,32 @@ - + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + { "associatedIndex": 6 @@ -65,6 +69,7 @@ + + - - - + + - + - + - + - + - - - - - + + + + + - - - - - + + + + + @@ -256,25 +268,51 @@ file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/test/test_pybullet.py - 29 + 33 + + file://$PROJECT_DIR$/soccer_perception/soccer_object_detection/test/test_object_detection.py + + + file://$PROJECT_DIR$/soccer_perception/soccer_object_detection/src/soccer_object_detection/camera/camera_calculations.py + 164 + + + file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py + 193 + + + file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py + 19 + + + + + + + + - - + + + - - - - + + + \ No newline at end of file diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py index 4aff137ae..26b4a4cee 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py @@ -1,3 +1,4 @@ +import enum from os.path import expanduser import yaml @@ -8,6 +9,13 @@ from soccer_common import Transformation +class BezStatusEnum(enum.IntEnum): + BALANCE = 0 + FIND_BALL = 1 + WALK = 2 + FALLEN = 3 + + class Bez: """ High level abstraction to represent the model. @@ -15,9 +23,16 @@ class Bez: # TODO should create more interfaces so that its easier to access objects, streamline interface - def __init__(self, robot_model: str = "bez1", pose: Transformation = Transformation(), fixed_base: bool = False): + def __init__( + self, + robot_model: str = "bez1", + pose: Transformation = Transformation(), + fixed_base: bool = False, + status: BezStatusEnum = BezStatusEnum.BALANCE, + ): self.robot_model = robot_model self.parameters = self.get_parameters() + self._status = status self.model = LoadModel(self.robot_model, self.parameters["walking_torso_height"], pose, fixed_base) @@ -25,6 +40,14 @@ def __init__(self, robot_model: str = "bez1", pose: Transformation = Transformat self.sensors = Sensors(self.model.body) + @property + def status(self) -> BezStatusEnum: + return self._status + + @status.setter + def status(self, status: BezStatusEnum) -> None: + self._status = status + def get_parameters(self) -> dict: with open( expanduser("~") @@ -46,3 +69,19 @@ def fallen(pitch: float) -> bool: print("Fallen Back") return True return False + + @property + def is_balance(self) -> bool: + return self.status == BezStatusEnum.BALANCE + + @property + def is_find_ball(self) -> bool: + return self.status == BezStatusEnum.FIND_BALL + + @property + def is_walk(self) -> bool: + return self.status == BezStatusEnum.WALK + + @property + def is_fallen(self) -> bool: + return self.status == BezStatusEnum.FALLEN 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 d9ec04eb6..261fae0f1 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py @@ -1,5 +1,6 @@ from typing import List +import cv2 import numpy as np import pybullet as pb @@ -43,6 +44,7 @@ def get_imu(self) -> Transformation: return Transformation(pos, orientation).orientation_euler def get_ball(self): + return Transformation() def get_foot_pressure_sensors(self, floor: pb.loadURDF) -> List[bool]: @@ -86,31 +88,30 @@ def get_camera_image(self): """ Captures the image from the camera mounted on the robot """ - robot_position, robot_orientation = pb.getBasePositionAndOrientation(self.body) - # Add more offsets later - camera_position = robot_position - camera_target = [robot_position[0] + 1, robot_position[1], robot_position[2]] + camera_position = self.get_pose(link=1).position + camera_target = [camera_position[0] + 1, camera_position[1], camera_position[2]] camera_up = [0, 0, 1] view_matrix = pb.computeViewMatrix(camera_position, camera_target, camera_up) - width, height = 1280, 720 - fov = 60 + width, height = 640, 480 + fov = 78 aspect = width / height - near = 0.1 - far = 50 + near = 0.2 + far = 100 projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, near, far) - images = pb.getCameraImage(width, height, view_matrix, projection_matrix, shadow=False, - renderer=pb.ER_BULLET_HARDWARE_OPENGL) + images = pb.getCameraImage(width, height, view_matrix, projection_matrix, shadow=False, renderer=pb.ER_BULLET_HARDWARE_OPENGL) # NOTE: the ordering of height and width change based on the conversion - rgb_opengl = np.reshape(images[2], (height, width, 4))[:, :, :3] * 1. / 255. + img = np.reshape(images[2], (height, width, 4)) + img = cv2.cvtColor(img, cv2.COLOR_RGBA2BGR) + # rgb_opengl = np.reshape(images[2], (height, width, 4))[:, :, :3] * 1. / 255. # depth_buffer_opengl = np.reshape(images[3], [width, height]) # depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl) # seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255. - return rgb_opengl \ No newline at end of file + return img diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py index af232796a..c5d1aeaad 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py @@ -54,6 +54,9 @@ def __init__( spinningFriction=spinning_friction, rollingFriction=rolling_friction, ) + ball = pb.loadURDF("soccerball.urdf", [1, 0, 0.1], globalScaling=0.14) + pb.changeDynamics(ball, -1, linearDamping=0, angularDamping=0, rollingFriction=0.001, spinningFriction=0.001) + pb.changeVisualShape(ball, -1, rgbaColor=[0.8, 0.8, 0.8, 1]) def close(self): if pb.isConnected(self.client_id): 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 aeadab658..26068fc91 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 @@ -110,10 +110,8 @@ def setup_tasks(self): ) joints_task.configure("joints", "soft", 1.0) - # self.look_at_ball = self.solver.add_axisalign_task( - # "camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0]) - # ) - # self.look_at_ball.configure("look_ball", "soft", 1) + self.look_at_ball = self.solver.add_axisalign_task("camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0])) + self.look_at_ball.configure("look_ball", "soft", 1) # Placing the robot in the initial position print("Placing the robot in the initial position...") 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 70a52371e..34779773d 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 @@ -13,6 +13,7 @@ from soccer_common import PID, Transformation + # TODO could make it more modular by passing in pybullet stuff or have it at one layer higher so we can reuse code # TODO change to trajectory controller class Navigator: @@ -142,7 +143,7 @@ def walk_pose(self, target_goal: Transformation): # dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2], curr_loc=position[:2]) # self.foot_step_planner.configure_planner(dx, dy) - t = self.walk_loop(t) + t = self.walk_loop(t) # TODO move main loop out of here def walk_ball(self, target_goal: Transformation): self.walk_pid.reset_imus() @@ -192,13 +193,13 @@ def walk_time(self, target_goal: list): self.walk_pid.reset_imus() t = 0 while t < target_goal[4]: - # self.foot_step_planner.head_movement(self.bez.sensors.get_ball().position) + # self.foot_step_planner.head_movement([1,1,0])#self.bez.sensors.get_ball().position) t = self.walk_loop(t) def walk_loop(self, t: float): self.foot_step_planner.plan_steps(t) self.bez.motor_control.configuration = self.filter_joints() - + # self.foot_step_planner.head_movement([1, 1, 0]) if self.imu_feedback_enabled and self.bez.sensors.imu_ready: [_, pitch, roll] = self.bez.sensors.get_imu() self.stabilize_walk(pitch, roll) diff --git a/soccer_control/soccer_pycontrol/test/test_placo.py b/soccer_control/soccer_pycontrol/test/test_placo.py index 0c9197619..e0d006537 100644 --- a/soccer_control/soccer_pycontrol/test/test_placo.py +++ b/soccer_control/soccer_pycontrol/test/test_placo.py @@ -30,7 +30,8 @@ def test_bez1(self): # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) print("STARTING WALK") walk.walk(target_goal, display_metrics=False) - # walk.wait(1000) + + walk.wait(1000) # TODO fix def test_bez2(self): diff --git a/soccer_control/soccer_pycontrol/test/test_pybullet.py b/soccer_control/soccer_pycontrol/test/test_pybullet.py index 8c29b47cf..a11c7452a 100644 --- a/soccer_control/soccer_pycontrol/test/test_pybullet.py +++ b/soccer_control/soccer_pycontrol/test/test_pybullet.py @@ -1,5 +1,7 @@ import unittest +from os.path import expanduser +import cv2 import numpy as np import pybullet as pb import pytest diff --git a/soccer_description/assembly_description/urdf/robot.urdf b/soccer_description/assembly_description/urdf/robot.urdf index 5be771b55..977ee3bbf 100644 --- a/soccer_description/assembly_description/urdf/robot.urdf +++ b/soccer_description/assembly_description/urdf/robot.urdf @@ -616,6 +616,20 @@ - + + + + + + + + + + + + + + + diff --git a/soccer_description/bez1_description/scripts/convert_stl2dae.py b/soccer_description/bez1_description/scripts/convert_stl2dae.py index da9199804..b144a4169 100644 --- a/soccer_description/bez1_description/scripts/convert_stl2dae.py +++ b/soccer_description/bez1_description/scripts/convert_stl2dae.py @@ -9,9 +9,9 @@ os.chdir(args.folder) for stl_fileName in glob.glob("*.stl"): - conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "obj" + conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "dae" os.system(conversion_command) for stl_fileName in glob.glob("*.STL"): - conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "obj" + conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "dae" os.system(conversion_command) diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/camera/camera_calculations.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/camera/camera_calculations.py index 08bec0d92..93adbf1ad 100644 --- a/soccer_perception/soccer_object_detection/src/soccer_object_detection/camera/camera_calculations.py +++ b/soccer_perception/soccer_object_detection/src/soccer_object_detection/camera/camera_calculations.py @@ -149,7 +149,7 @@ def calculate_bounding_boxes_from_ball(self, ball_position: Transformation, ball return bounding_box - def calculate_ball_from_bounding_boxes(self, ball_radius: float = 0.07, bounding_boxes: [float] = []) -> Transformation: + def calculate_ball_from_bounding_boxes(self, bounding_boxes: [float] = [], ball_radius: float = 0.07) -> Transformation: """ Reverse function for :func:`~soccer_common.Camera.calculateBoundingBoxesFromBall`, takes the bounding boxes of the ball as seen on the camera and return the 3D position of the ball assuming that the ball is on the ground diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py index 3cf7040e2..c3aeabc38 100755 --- a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py +++ b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node.py @@ -64,8 +64,8 @@ def get_model_output(self, image: Mat) -> [Mat, BoundingBoxes]: # cover horizon to help robot ignore things outside field # TODO do we need a cover horizon? - h = max(self.camera.calculate_horizon_cover_area() - self.cover_horizon_up_threshold, 0) - # h = 0 + # h = max(self.camera.calculate_horizon_cover_area() - self.cover_horizon_up_threshold, 0) + h = 0 if image is not None: # 1. preprocess image img = image[:, :, :3] # get rid of alpha channel diff --git a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py index f1781cbbf..6fd62f326 100755 --- a/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py +++ b/soccer_perception/soccer_object_detection/src/soccer_object_detection/object_detect_node_ros.py @@ -92,7 +92,7 @@ def callback(self, msg: Image): for box in bbs_msg.bounding_boxes: if box.Class == "0": boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] - ball_pose = self.camera.calculate_ball_from_bounding_boxes(0.07, boundingBoxes) + ball_pose = self.camera.calculate_ball_from_bounding_boxes(boundingBoxes) self.br2.sendTransform( ball_pose.position, ball_pose.quaternion, diff --git a/soccer_perception/soccer_object_detection/test/test_camera.py b/soccer_perception/soccer_object_detection/test/test_camera.py index c98c28aca..6fe66d849 100644 --- a/soccer_perception/soccer_object_detection/test/test_camera.py +++ b/soccer_perception/soccer_object_detection/test/test_camera.py @@ -68,9 +68,9 @@ def test_calculate_bounding_boxes_from_ball(self): ball_pose = Transformation(position) ball_radius = 0.07 - bounding_boxes = c.calculate_bounding_boxes_from_ball(ball_pose, ball_radius) + bounding_boxes = c.calculate_bounding_boxes_from_ball(ball_pose, ball_radius) # TODO fix return format # [[135.87634651355825, 75.87634651355823], [224.12365348644175, 164.12365348644175]] - position = c.calculate_ball_from_bounding_boxes(ball_radius, bounding_boxes) + position = c.calculate_ball_from_bounding_boxes(bounding_boxes, ball_radius) # TODO fix # self.assertAlmostEqual(position.position[0], ball_pose.position[0], delta=0.001) # self.assertAlmostEqual(position.position[1], ball_pose.position[1], delta=0.001) diff --git a/soccer_perception/soccer_object_detection/test/test_pybullet_camera.py b/soccer_perception/soccer_object_detection/test/test_pybullet_camera.py new file mode 100644 index 000000000..4424bbf1f --- /dev/null +++ b/soccer_perception/soccer_object_detection/test/test_pybullet_camera.py @@ -0,0 +1,115 @@ +import os +import unittest +from os.path import expanduser + +import cv2 +import numpy as np +import pybullet as pb +import pytest +from soccer_object_detection.object_detect_node import ObjectDetectionNode +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld + +from soccer_common import Transformation + +REAL_TIME = True +PLOT = True + + +class TestPybullet(unittest.TestCase): + def tearDown(self): + self.world.close() + del self.bez + del self.world + + def test_cam(self): + + self.world = PybulletWorld(camera_yaw=45, real_time=REAL_TIME, rate=100) + pose = Transformation() + self.bez = Bez(robot_model="assembly", pose=pose) + for i in range(100): + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + + if "DISPLAY" in os.environ and PLOT: + cv2.imshow("CVT Color2", img) + cv2.waitKey(1) + + self.world.step() + + def test_detection(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + + self.world = PybulletWorld(camera_yaw=45, real_time=REAL_TIME, rate=100) + pose = Transformation() + self.bez = Bez(robot_model="assembly", pose=pose) + + self.world.wait(100) + for i in range(100): + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs = detect.get_model_output(img) + if "DISPLAY" in os.environ and PLOT: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + self.world.step() + + def test_ball_localization(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + + self.world = PybulletWorld(camera_yaw=45, real_time=REAL_TIME, rate=100) + pose = Transformation() + self.bez = Bez(robot_model="assembly", pose=pose) + + for i in range(1000): + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs_msg = detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + detect.camera.pose = self.bez.sensors.get_pose(link=2) + boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + print(detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + if "DISPLAY" in os.environ and PLOT: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + self.world.step() + + def test_ball_localization2(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + self.world = PybulletWorld(camera_yaw=45, real_time=REAL_TIME, rate=100) + pose = Transformation() + self.bez = Bez(robot_model="assembly", pose=pose) + + for i in range(1000): + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs_msg = detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + pos = [box.xbase, box.ybase] + floor_coordinate_robot = detect.camera.find_floor_coordinate(pos) + world_to_obstacle = Transformation(position=floor_coordinate_robot) + detect.camera.pose = self.bez.sensors.get_pose(link=1) + # detect.camera.pose.orientation_euler = [0, 0, 0] + camera_to_obstacle = np.linalg.inv(detect.camera.pose) @ world_to_obstacle + print(camera_to_obstacle.position, " ", self.bez.sensors.get_pose(link=1).position) + if "DISPLAY" in os.environ and PLOT: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + self.world.step() diff --git a/soccer_strategy/src/soccer_strategy/behavior/behavior.py b/soccer_strategy/src/soccer_strategy/behavior/behavior.py index f38ccd0d5..4324a281c 100644 --- a/soccer_strategy/src/soccer_strategy/behavior/behavior.py +++ b/soccer_strategy/src/soccer_strategy/behavior/behavior.py @@ -2,6 +2,10 @@ from abc import ABC, abstractmethod +from soccer_object_detection.object_detect_node import ObjectDetectionNode +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.walk_engine.navigator import Navigator + class Behavior(ABC): """ @@ -41,3 +45,19 @@ def state(self) -> Behavior: @state.setter def state(self, state) -> None: self._context.state = state + + @property + def bez(self) -> Bez: + return self._context.bez # type: ignore[no-any-return] + + @property + def world(self) -> world: + return self._context.world # type: ignore[no-any-return] + + @property + def nav(self) -> Navigator: + return self._context.nav # type: ignore[no-any-return] + + @property + def detect(self) -> ObjectDetectionNode: + return self._context.detect # type: ignore[no-any-return] diff --git a/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py b/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py index 7949d518a..3d135e529 100644 --- a/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py +++ b/soccer_strategy/src/soccer_strategy/behavior/behavior_context.py @@ -1,5 +1,12 @@ from __future__ import annotations +from os.path import expanduser + +from soccer_object_detection.object_detect_node import ObjectDetectionNode +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld +from soccer_pycontrol.walk_engine.navigator import Navigator + from soccer_strategy.behavior import Behavior from soccer_strategy.behavior.state.balance import Balance @@ -17,7 +24,12 @@ class BehaviorContext: A reference to the current state of the BehaviorContext. """ - def __init__(self, sim: bool = True) -> None: + def __init__(self, world: PybulletWorld, bez: Bez, nav: Navigator, detect: ObjectDetectionNode, sim: bool = True) -> None: + self.world = world + self.bez = bez + self.nav = nav + self.detect = detect + self.sim = sim # TODO clean up self.transition_to(Balance()) # Has to be last. setting context for current state diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/balance.py b/soccer_strategy/src/soccer_strategy/behavior/state/balance.py index fc0f142e2..4bcee186d 100644 --- a/soccer_strategy/src/soccer_strategy/behavior/state/balance.py +++ b/soccer_strategy/src/soccer_strategy/behavior/state/balance.py @@ -1,9 +1,11 @@ +from soccer_pycontrol.model.bez import BezStatusEnum + from soccer_strategy.behavior import Behavior class Balance(Behavior): def action(self) -> None: - pass + self.bez.status = BezStatusEnum.BALANCE def run_algorithim(self) -> None: pass diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/fallen.py b/soccer_strategy/src/soccer_strategy/behavior/state/fallen.py new file mode 100644 index 000000000..a5d76e5e3 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/state/fallen.py @@ -0,0 +1,14 @@ +from soccer_pycontrol.model.bez import BezStatusEnum + +from soccer_strategy.behavior import Behavior + + +class Balance(Behavior): + def action(self) -> None: + self.bez.status = BezStatusEnum.FALLEN + + def run_algorithim(self) -> None: + pass + + def ready_to_switch_to(self) -> bool: + return True diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/find_ball.py b/soccer_strategy/src/soccer_strategy/behavior/state/find_ball.py new file mode 100644 index 000000000..fa843ccdf --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/state/find_ball.py @@ -0,0 +1,31 @@ +import os + +import cv2 +from soccer_pycontrol.model.bez import BezStatusEnum + +from soccer_strategy.behavior import Behavior + +PLOT = True + + +class FindBall(Behavior): + def action(self) -> None: + self.bez.status = BezStatusEnum.FIND_BALL + + def run_algorithim(self) -> None: + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs_msg = self.detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + self.detect.camera.pose = self.bez.sensors.get_pose(link=2) + boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + print(self.detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + + if "DISPLAY" in os.environ and PLOT: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + def ready_to_switch_to(self) -> bool: + return True diff --git a/soccer_strategy/src/soccer_strategy/behavior/state/walk.py b/soccer_strategy/src/soccer_strategy/behavior/state/walk.py new file mode 100644 index 000000000..4411b4574 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior/state/walk.py @@ -0,0 +1,14 @@ +from soccer_pycontrol.model.bez import BezStatusEnum + +from soccer_strategy.behavior import Behavior + + +class Walk(Behavior): + def action(self) -> None: + self.bez.status = BezStatusEnum.WALK + + def run_algorithim(self) -> None: + pass + + def ready_to_switch_to(self) -> bool: + return True diff --git a/soccer_strategy/src/soccer_strategy/behavior_executive.py b/soccer_strategy/src/soccer_strategy/behavior_executive.py index 9d5bb6ea9..ddc6c3ee8 100644 --- a/soccer_strategy/src/soccer_strategy/behavior_executive.py +++ b/soccer_strategy/src/soccer_strategy/behavior_executive.py @@ -1,15 +1,17 @@ #!/usr/bin/env python3 import os +from os.path import expanduser import rospy -from evtol_behavior.autopilot_context_ros import AutoPilotContextRos -from evtol_behavior.behavior.state.land import Land -from evtol_behavior.behavior_context_ros import BehaviorContextRos -from evtol_behavior.health_system import HealthSystem -from evtol_common.drone_ros import DroneRos # type: ignore[attr-defined] -from evtol_msgs.msg import DroneStatus # type: ignore[attr-defined] -from std_msgs.msg import Header -from std_srvs.srv import SetBool, SetBoolRequest +from soccer_object_detection.object_detect_node import ObjectDetectionNode +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld +from soccer_pycontrol.walk_engine.navigator import Navigator + +from soccer_common import Transformation +from soccer_strategy.behavior.behavior_context import BehaviorContext + +REAL_TIME = True class BehaviorExecutive: @@ -20,27 +22,20 @@ class BehaviorExecutive: def __init__(self): # Initialize node - rospy.init_node("evtol_behavior") - - # Initialize attributes - # TODO tests fail if 30 hz, that shouldnt hapen - self.rate = rospy.Rate(rospy.get_param("/evtol_nav/rate", 20)) - self.sim = rospy.get_param("/simulation", os.environ.get("SIM", False)) - self.last_req = rospy.Time.now() - self._drone = DroneRos() - # TODO limit drone nand path to pass from here during loop - self._behavior = BehaviorContextRos(self._drone, self.sim) # TODO clean up - self._autopilot = AutoPilotContextRos(self._behavior) - self._health_system = HealthSystem() + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + self.nav = Navigator(self.world, self.bez, imu_feedback_enabled=False) - # Publisher - self._drone_status_pub = rospy.Publisher("evtol_behavior/drone_status", DroneStatus, queue_size=10) + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + self.detect = ObjectDetectionNode(model_path) # TODO should have a switch - # TODO add uwb to sim - if not self.sim: - rospy.wait_for_service("/evtol_sensors/uwb/enable") - self.srv = rospy.ServiceProxy("evtol_sensors/uwb/enable", SetBool) + self.behavior = BehaviorContext(world=self.world, bez=self.bez, nav=self.nav, detect=self.detect) # Main communication node for ground control def run(self): @@ -54,21 +49,10 @@ def run(self): while not rospy.is_shutdown(): # Behaviour Executive # TODO pass drone & path harder then previously thought might be possible but not worth time rigth now - self._behavior.run_state_algorithim() - - # Health System TODO fix starting issue and maybe but this - if self._health_system.check_health(self._drone.z - self._drone.disarm_height): - if not self.sim: - self.srv.call(SetBoolRequest(data=True)) - rospy.loginfo_throttle(1, "switching to uwb") - self._behavior.state = Land() + self.behavior.run_state_algorithim() # AutoPilot - # TODO pass behavior - self._autopilot.check_autopilot() - - # TODO put in drone maybe? - msg = DroneStatus(header=Header(stamp=rospy.Time.now(), frame_id="map"), data=self._drone.status) - self._drone_status_pub.publish(msg) + # # TODO pass behavior + # self._autopilot.check_autopilot() self.rate.sleep() diff --git a/soccer_strategy/src/soccer_strategy/behavior_executive_ros.py b/soccer_strategy/src/soccer_strategy/behavior_executive_ros.py new file mode 100644 index 000000000..7b683ca85 --- /dev/null +++ b/soccer_strategy/src/soccer_strategy/behavior_executive_ros.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +import os + +import rospy +from evtol_behavior.autopilot_context_ros import AutoPilotContextRos +from evtol_behavior.behavior.state.land import Land +from evtol_behavior.behavior_context_ros import BehaviorContextRos +from evtol_behavior.health_system import HealthSystem +from evtol_common.drone_ros import DroneRos # type: ignore[attr-defined] +from evtol_msgs.msg import DroneStatus # type: ignore[attr-defined] +from std_msgs.msg import Header +from std_srvs.srv import SetBool, SetBoolRequest + + +class BehaviorExecutiveRos: + """ + This class is responsible for the main decision-making of the evtol and uses all systems to control the evtol. + Integration and delegation for other modules. Code for any decision drone has to make. + """ + + def __init__(self): + # Initialize node + rospy.init_node("evtol_behavior") + + # Initialize attributes + # TODO tests fail if 30 hz, that shouldnt hapen + self.rate = rospy.Rate(rospy.get_param("/evtol_nav/rate", 20)) + self.sim = rospy.get_param("/simulation", os.environ.get("SIM", False)) + self.last_req = rospy.Time.now() + + self._drone = DroneRos() + # TODO limit drone nand path to pass from here during loop + self._behavior = BehaviorContextRos(self._drone, self.sim) # TODO clean up + self._autopilot = AutoPilotContextRos(self._behavior) + self._health_system = HealthSystem() + + # Publisher + self._drone_status_pub = rospy.Publisher("evtol_behavior/drone_status", DroneStatus, queue_size=10) + + # TODO add uwb to sim + if not self.sim: + rospy.wait_for_service("/evtol_sensors/uwb/enable") + self.srv = rospy.ServiceProxy("evtol_sensors/uwb/enable", SetBool) + + # Main communication node for ground control + def run(self): + """ + Main loop + + :return: None + """ + + # Main loop to follow waypoints + while not rospy.is_shutdown(): + # Behaviour Executive + # TODO pass drone & path harder then previously thought might be possible but not worth time rigth now + self._behavior.run_state_algorithim() + + # Health System TODO fix starting issue and maybe but this + if self._health_system.check_health(self._drone.z - self._drone.disarm_height): + if not self.sim: + self.srv.call(SetBoolRequest(data=True)) + rospy.loginfo_throttle(1, "switching to uwb") + self._behavior.state = Land() + + # AutoPilot + # TODO pass behavior + self._autopilot.check_autopilot() + + # TODO put in drone maybe? + msg = DroneStatus(header=Header(stamp=rospy.Time.now(), frame_id="map"), data=self._drone.status) + self._drone_status_pub.publish(msg) + + self.rate.sleep() From cae3918fbc405b8bec4ffc82c33049ab16f8503b Mon Sep 17 00:00:00 2001 From: manx52 Date: Thu, 5 Dec 2024 18:54:11 -0500 Subject: [PATCH 02/24] moved ball to where bez is loaded. added a groundtruth get ball. speed up pybullet. properly enable head movement. tuned pid for nav on the new robot. created a new test that has nav without it owning the loops so we can add other operations. --- .idea/workspace.xml | 102 +++++------ .../src/soccer_pycontrol/model/bez.py | 2 +- .../src/soccer_pycontrol/model/sensors.py | 9 +- .../pybullet_usage/pybullet_load_model.py | 6 +- .../pybullet_usage/pybullet_world.py | 12 +- .../walk_engine/foot_step_planner.py | 10 +- .../soccer_pycontrol/walk_engine/navigator.py | 165 ++++++++++-------- .../soccer_pycontrol/test/test_placo.py | 104 +++++------ 8 files changed, 206 insertions(+), 204 deletions(-) diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 75dc21c37..bbf601937 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -5,32 +5,16 @@ - - - - - + - - - - - - - - - - - - - { + "keyToString": { + "Python tests.pytest for test_camera.TestCamera.test_camera_find_camera_coordinate.executor": "Run", + "Python tests.pytest for test_firmware_interface.test_firmware_interface.executor": "Run", + "Python tests.pytest for test_firmware_interface.test_firmware_interface_single_motor_range.executor": "Run", + "Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection.executor": "Run", + "Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection_vid.executor": "Run", + "Python tests.pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid.executor": "Run", + "Python tests.pytest for test_placo.TestPlaco.test_bez1.executor": "Run", + "Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range.executor": "Run", + "Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range_single.executor": "Run", + "Python tests.pytest for test_pybullet.TestPybullet.test_cam.executor": "Run", + "Python tests.pytest for test_pybullet.TestPybullet.test_imu.executor": "Debug", + "Python tests.pytest for test_pybullet_camera.TestPybullet.test_ball_localization.executor": "Run", + "Python tests.pytest for test_pybullet_camera.TestPybullet.test_ball_localization2.executor": "Run", + "Python tests.pytest for test_pybullet_camera.TestPybullet.test_cam.executor": "Run", + "Python tests.pytest for test_pybullet_camera.TestPybullet.test_detection.executor": "Run", + "Python tests.pytest for test_trajectory.TestTrajectory.test_trajectory.executor": "Run", + "Python tests.pytest for test_trajectory.test_trajectory_sim.executor": "Run", + "Python tests.pytest for test_walk_ros.TestPybullet.test_walk_ros_local.executor": "Run", + "Python.dgfs.executor": "Run", + "Python.fsf.executor": "Run", + "RunOnceActivity.ShowReadmeOnStart": "true", + "git-widget-placeholder": "js__strat", + "last_opened_file_path": "/home/jonathan/catkin_ws/src/soccerbot/soccer_strategy/src/soccer_strategy", + "node.js.detected.package.eslint": "true", + "node.js.detected.package.tslint": "true", + "node.js.selected.package.eslint": "(autodetect)", + "node.js.selected.package.tslint": "(autodetect)", + "nodejs_package_manager_path": "npm", + "settings.editor.selected.configurable": "preferences.fileTypes", + "vue.rearranger.settings.migration": "true" } -}]]> +} @@ -142,7 +126,7 @@ @@ -268,7 +255,7 @@ file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/test/test_pybullet.py - 33 + 31 @@ -281,15 +268,10 @@ - file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py - 193 + file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py + 48 - - file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py - 19 - @@ -313,6 +295,6 @@ - + \ No newline at end of file diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py index 26b4a4cee..88667a750 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py @@ -38,7 +38,7 @@ def __init__( self.motor_control = MotorControl(self.model.body) - self.sensors = Sensors(self.model.body) + self.sensors = Sensors(self.model.body, self.model.ball) @property def status(self) -> BezStatusEnum: 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 261fae0f1..1a02d5c88 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py @@ -12,9 +12,10 @@ class Sensors: Interfaces with pybullet to extract sensor data. """ - def __init__(self, body: pb.loadURDF): + def __init__(self, body: pb.loadURDF, ball: pb.loadURDF): # TODO does this need to be a class? self.body = body + self.ball = ball # TODO should get based on name self.imu_link = pb.getNumJoints(self.body) - 1 self.imu_ready = False @@ -44,8 +45,10 @@ def get_imu(self) -> Transformation: return Transformation(pos, orientation).orientation_euler def get_ball(self): - - return Transformation() + [position, quaternion] = pb.getBasePositionAndOrientation(self.body) + [ball_position, ball_quaternion] = pb.getBasePositionAndOrientation(self.ball) + trans = (np.array(ball_position) - np.array(position)).tolist() + return Transformation(position=trans, quaternion=ball_quaternion) def get_foot_pressure_sensors(self, floor: pb.loadURDF) -> List[bool]: """ diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py index ac486bb78..b9a2e0a1c 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py @@ -22,12 +22,16 @@ def __init__(self, robot_model: str, walking_torso_height: float, pose: Transfor if not fixed_base and (self.pose == Transformation()).all(): # TODO need way to have custom height self.set_pose(pose) + self.ball = pb.loadURDF("soccerball.urdf", [1, 0, 0.1], globalScaling=0.14) + pb.changeDynamics(self.ball, -1, linearDamping=0, angularDamping=0, rollingFriction=0.001, spinningFriction=0.001) + pb.changeVisualShape(self.ball, -1, rgbaColor=[0.8, 0.8, 0.8, 1]) + def load_urdf_pybullet(self, urdf_model_path: str, fixed_base: bool): # -> pb.loadURDF: # TODO read from yaml? Also maybe put in world body = pb.loadURDF( urdf_model_path, useFixedBase=fixed_base, - flags=pb.URDF_USE_INERTIA_FROM_FILE | 0, + flags=pb.URDF_USE_INERTIA_FROM_FILE | pb.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | 0, basePosition=self.pose.position, baseOrientation=self.pose.quaternion, ) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py index c5d1aeaad..1b72660fa 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py @@ -32,17 +32,22 @@ def __init__( """ self.rate = rate self.real_time = real_time - + optionstring = "--width={} --height={} ".format(640, 480) assert pb.isConnected() == 0 if display: - self.client_id = pb.connect(pb.GUI) + self.client_id = pb.connect(pb.GUI, optionstring) else: self.client_id = pb.connect(pb.DIRECT) + # pb.setTimeStep(0.005) + pb.setRealTimeSimulation(0) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally pb.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=camera_yaw, cameraPitch=0, cameraTargetPosition=cameraTargetPosition) pb.setGravity(0, 0, -9.81) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) if path != "": self.plane = pb.loadURDF(path, basePosition=position, baseOrientation=pb.getQuaternionFromEuler(orientation)) @@ -54,9 +59,6 @@ def __init__( spinningFriction=spinning_friction, rollingFriction=rolling_friction, ) - ball = pb.loadURDF("soccerball.urdf", [1, 0, 0.1], globalScaling=0.14) - pb.changeDynamics(ball, -1, linearDamping=0, angularDamping=0, rollingFriction=0.001, spinningFriction=0.001) - pb.changeVisualShape(ball, -1, rgbaColor=[0.8, 0.8, 0.8, 1]) def close(self): if pb.isConnected(self.client_id): 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 26068fc91..089b7c145 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 @@ -91,8 +91,8 @@ def setup_tasks(self): "right_shoulder_roll": -shoulder_roll, "right_shoulder_pitch": shoulder_pitch, "right_elbow": elbow, - "head_pitch": 0.0, - "head_yaw": 0.0, + # "head_pitch": 0.0, + # "head_yaw": 0.0, } ) else: @@ -104,14 +104,14 @@ def setup_tasks(self): # "right_shoulder_roll": -shoulder_roll, "right_shoulder_pitch": shoulder_pitch, "right_elbow": elbow, - "head_pitch": 0.0, - "head_yaw": 0.0, + # "head_pitch": 0.0, + # "head_yaw": 0.0, } ) joints_task.configure("joints", "soft", 1.0) self.look_at_ball = self.solver.add_axisalign_task("camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0])) - self.look_at_ball.configure("look_ball", "soft", 1) + self.look_at_ball.configure("look_ball", "soft", 1) # TODO replace with a function that remove_task # Placing the robot in the initial position print("Placing the robot in the initial position...") 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 34779773d..9c3b7e541 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 @@ -25,7 +25,7 @@ def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, time.time) self.walk_pid = Stabilize(self.bez.parameters) - self.max_vel = 0.02 + self.max_vel = 0.1 self.nav_x_pid = PID( Kp=0.5, Kd=0, @@ -34,22 +34,22 @@ def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = output_limits=(-self.max_vel, self.max_vel), ) self.nav_y_pid = PID( # TODO properly tune later - Kp=0.1, + Kp=0.5, Kd=0, Ki=0, setpoint=0, - output_limits=(-self.max_vel + 0.01, self.max_vel), + output_limits=(-self.max_vel, self.max_vel), ) # TODO could also mod if balance is decreasing self.nav_yaw_pid = PID( - Kp=0.05, + Kp=0.2, Kd=0, Ki=0, setpoint=0, output_limits=(-1, 1), ) - self.error_tol = 0.01 # in m TODO add as a param and in the ros version + self.error_tol = 0.03 # in m TODO add as a param and in the ros version # joints self.left_ankle_index = self.bez.motor_control.motor_names.index("left_ankle_roll") @@ -58,20 +58,28 @@ def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = self.record_walking_metrics = record_walking_metrics self.walking_data = defaultdict(list) + self.t = None + self.enable_walking = None + self.reset_walk() + + def reset_walk(self): + self.t = -1 + self.enable_walking = True # TODO could make input a vector def walk(self, target_goal: Union[Transformation, List], ball_mode: bool = False, display_metrics: bool = False): - if isinstance(target_goal, Transformation): - if ball_mode: - self.walk_ball(target_goal) - else: - self.walk_pose(target_goal) - elif isinstance(target_goal, list): # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10] - self.walk_time(target_goal) + if self.enable_walking: + if isinstance(target_goal, Transformation): + if ball_mode: + self.walk_ball(target_goal) + else: + self.walk_pose(target_goal) + elif isinstance(target_goal, list): # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10] + self.walk_time(target_goal) # if self.record_walking_metrics and display_metrics: # self.display_walking_metrics(show_targets=isinstance(target_goal, Transformation)) - self.ready() + # self.ready() def find_new_vel(self, goal_loc: list, curr_loc: list = (0, 0)): x_error = abs(goal_loc[0] - curr_loc[0]) @@ -90,27 +98,29 @@ def find_new_vel(self, goal_loc: list, curr_loc: list = (0, 0)): return math.copysign(dx, goal_loc[0]), math.copysign(dy, goal_loc[1]) def walk_pose(self, target_goal: Transformation): - self.walk_pid.reset_imus() - self.nav_x_pid.reset() - self.nav_x_pid.setpoint = target_goal.position[0] + pose = self.bez.sensors.get_pose() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) + + if self.t < 0: + self.walk_pid.reset_imus() + self.nav_x_pid.reset() + self.nav_x_pid.setpoint = target_goal.position[0] - self.nav_y_pid.reset() - self.nav_y_pid.setpoint = target_goal.position[1] + self.nav_y_pid.reset() + self.nav_y_pid.setpoint = target_goal.position[1] - self.nav_yaw_pid.reset() - self.nav_yaw_pid.setpoint = target_goal.orientation_euler[0] + self.nav_yaw_pid.reset() + self.nav_yaw_pid.setpoint = target_goal.orientation_euler[0] - pose = self.bez.sensors.get_pose() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) - dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) - # he = self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0]) - # dtheta = math.copysign(0.5, he) - self.foot_step_planner.setup_walk(dx, dy) - t = 0 - # TODO fix and add to a nav could add a funct for pybullet or python - # TODO could have a balancing mode by default could use the COM - # TODO for ball could just remove - - while ( + dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) + # he = self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0]) + # dtheta = math.copysign(0.5, he) + self.foot_step_planner.setup_walk(dx, dy) + self.t = 0 + # TODO fix and add to a nav could add a funct for pybullet or python + # TODO could have a balancing mode by default could use the COM + # TODO for ball could just remove + + if ( self.position_error(pose.position[:2], target_goal.position[:2]) > self.error_tol or abs(self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0])) > self.error_tol ): # self.bez.sensors.get_pose() #TODO about 20% or 40% error @@ -119,7 +129,7 @@ def walk_pose(self, target_goal: Transformation): ) # self.foot_step_planner.robot.get_T_world_trunk() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) # TODO should be broken up and a unit test - print(self.foot_step_planner.robot.com_world()) + # print(self.foot_step_planner.robot.com_world()) goal = Transformation() goal.rotation_matrix = np.matmul(target_goal.rotation_matrix, scipy.linalg.inv(pose.rotation_matrix)) goal.position = pose.rotation_matrix.T @ target_goal.position - pose.rotation_matrix.T @ pose.position @@ -136,43 +146,40 @@ def walk_pose(self, target_goal: Transformation): dtheta = self.nav_yaw_pid.update(pose.orientation_euler[0]) 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) + self.walk_loop() # TODO move main loop out of here + else: + self.ready() + self.enable_walking = False - # if t > 5: - # - # target_goal = Transformation(position=[-0.5,-0.5,0]) - # dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2], curr_loc=position[:2]) - # self.foot_step_planner.configure_planner(dx, dy) + def walk_ball(self, target_goal: Transformation): + if self.t < 0: + self.walk_pid.reset_imus() + self.nav_x_pid.reset() + self.nav_x_pid.setpoint = target_goal.position[0] - t = self.walk_loop(t) # TODO move main loop out of here + self.nav_y_pid.reset() + self.nav_y_pid.setpoint = target_goal.position[1] - def walk_ball(self, target_goal: Transformation): - self.walk_pid.reset_imus() - self.nav_x_pid.reset() - self.nav_x_pid.setpoint = target_goal.position[0] - - self.nav_y_pid.reset() - self.nav_y_pid.setpoint = target_goal.position[1] - - self.nav_yaw_pid.reset() - self.nav_yaw_pid.setpoint = 0 # TODO add yaw modes - - dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) - # dx, dy = 0, 0 - self.foot_step_planner.setup_walk(dx, dy) - t = 0 - # TODO fix and add to a nav could add a funct for pybullet or python - # TODO could have a balancing mode by default could use the COM - # TODO for ball could just remove - while ( + self.nav_yaw_pid.reset() + self.nav_yaw_pid.setpoint = 0 # TODO add yaw modes + + dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) + # dx, dy = 0, 0 + self.foot_step_planner.setup_walk(dx, dy) + self.t = 0 + # TODO fix and add to a nav could add a funct for pybullet or python + # TODO could have a balancing mode by default could use the COM + # TODO for ball could just remove + if ( self.position_error(target_goal.position[:2]) > self.error_tol or abs(self.heading_error(target_goal.orientation_euler[0], self.bez.sensors.get_pose().orientation_euler[0])) > self.error_tol ): - target_goal = self.bez.sensors.get_ball() - - # self.foot_step_planner.head_movement(target_goal.position) + # target_goal = self.bez.sensors.get_ball() + # print(target_goal.position) + self.foot_step_planner.head_movement(target_goal.position) - self.nav_x_pid.setpoint = target_goal.position[0] - self.nav_y_pid.setpoint = target_goal.position[1] + self.nav_x_pid.setpoint = 0 # target_goal.position[0] + self.nav_y_pid.setpoint = 0 # target_goal.position[1] x_error = target_goal.position[0] y_error = target_goal.position[1] head_error = self.heading_error(target_goal.orientation_euler[0], self.bez.sensors.get_pose().orientation_euler[0]) @@ -183,21 +190,28 @@ def walk_ball(self, target_goal: Transformation): dy = self.nav_y_pid.update(0) dtheta = self.nav_yaw_pid.update(self.bez.sensors.get_pose().orientation_euler[0]) - print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3)) + # 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) - t = self.walk_loop(t) + self.walk_loop() + else: + self.ready() + self.enable_walking = False def walk_time(self, target_goal: list): - self.foot_step_planner.setup_walk(target_goal[0], target_goal[1], target_goal[2], target_goal[3]) - self.walk_pid.reset_imus() - t = 0 - while t < target_goal[4]: - # self.foot_step_planner.head_movement([1,1,0])#self.bez.sensors.get_ball().position) - t = self.walk_loop(t) - - def walk_loop(self, t: float): - self.foot_step_planner.plan_steps(t) + if self.t < 0: + self.foot_step_planner.setup_walk(target_goal[0], target_goal[1], target_goal[2], target_goal[3]) + self.walk_pid.reset_imus() + self.t = 0 + + if self.t < target_goal[4]: + self.walk_loop() + elif target_goal[4] <= self.t: + self.ready() + self.enable_walking = False + + def walk_loop(self): + self.foot_step_planner.plan_steps(self.t) self.bez.motor_control.configuration = self.filter_joints() # self.foot_step_planner.head_movement([1, 1, 0]) if self.imu_feedback_enabled and self.bez.sensors.imu_ready: @@ -205,9 +219,8 @@ def walk_loop(self, t: float): self.stabilize_walk(pitch, roll) self.bez.motor_control.set_motor() - self.func_step() - t = self.foot_step_planner.step(t) + self.t = self.foot_step_planner.step(self.t) # update joints in control # TODO investigate it has an effect but not sure how much also with step time # for joint in self.bez.motor_control.motor_names: @@ -222,8 +235,6 @@ def walk_loop(self, t: float): # if self.record_walking_metrics: # self.update_walking_metrics(t) - return t - @staticmethod def heading_error(theta_desired: float, theta_current: float) -> float: """ diff --git a/soccer_control/soccer_pycontrol/test/test_placo.py b/soccer_control/soccer_pycontrol/test/test_placo.py index e0d006537..e2f51542f 100644 --- a/soccer_control/soccer_pycontrol/test/test_placo.py +++ b/soccer_control/soccer_pycontrol/test/test_placo.py @@ -1,5 +1,10 @@ +import os import unittest +from os.path import expanduser +from random import uniform +import cv2 +from soccer_object_detection.object_detect_node import ObjectDetectionNode from soccer_pycontrol.model.bez import Bez from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld from soccer_pycontrol.walk_engine.navigator import Navigator @@ -16,33 +21,51 @@ def tearDown(self): del self.world def test_bez1(self): + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) self.world = PybulletWorld( camera_yaw=90, real_time=REAL_TIME, rate=200, ) - self.bez = Bez(robot_model="assembly", pose=Transformation()) + # self.bez = Bez(robot_model="assembly", pose=Transformation()) + self.bez = Bez(robot_model="bez1", pose=Transformation()) walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) - # walk.ready() - # self.bez.motor_control.set_motor() - # walk.wait(50) - target_goal = [1, 0, 0.0, 10, 500] - # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) + walk.ready() + walk.wait(100) + target_goal = [1, 0, 0, 10, 2] + target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) print("STARTING WALK") - walk.walk(target_goal, display_metrics=False) - - walk.wait(1000) + ball_pos = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + for i in range(10000): + if i % 10 == 0: + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs_msg = detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + detect.camera.pose = self.bez.sensors.get_pose(link=2) + boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + print(detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + ball_pos = detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes) + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) - # TODO fix - def test_bez2(self): - self.world = PybulletWorld( - camera_yaw=90, - real_time=REAL_TIME, - rate=200, - ) - self.bez = Bez(robot_model="bez2", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.walk(d_x=0.03, t_goal=10) + walk.walk(ball_pos, True) + # walk.walk(target_goal, display_metrics=False) + # if not walk.enable_walking: + # print("WALK ENABLED") + # x = uniform(-1, 1) # TODO own unit test for yaw + # y = uniform(-1, 1) + # theta = uniform(-3.14, 3.14) + # print(x, y, theta) + # target_goal = Transformation(position=[x, y, 0], euler=[theta, 0, 0]) + # walk.reset_walk() + self.world.step() def test_bez1_start_stop(self): self.world = PybulletWorld( @@ -50,41 +73,18 @@ def test_bez1_start_stop(self): real_time=REAL_TIME, rate=200, ) - self.bez = Bez(robot_model="bez1", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.walk(d_x=0.03, t_goal=5) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + walk.ready() walk.wait(100) - walk.walk(d_x=0.03, t_goal=5) - walk.wait(500) - - def test_bez1_replan(self): - self.world = PybulletWorld( - camera_yaw=90, - real_time=REAL_TIME, - rate=200, - ) - self.bez = Bez(robot_model="bez1", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.foot_step_planner.setup_walk(d_x=0.03) - walk.pid.reset_imus() - t = 0 - while t < 15: # TODO needs end condition - [_, pitch, roll] = walk.bez.sensors.get_imu() - if t > 3: - walk.foot_step_planner.configure_planner(d_y=0.03) - if t > 6: - walk.foot_step_planner.configure_planner(d_x=0.0, d_theta=0.4) - if t > 9: - walk.foot_step_planner.configure_planner(d_x=0.03, d_y=0.03) - walk.foot_step_planner.plan_steps(t) - - walk.bez.motor_control.configuration = walk.filter_joints() - walk.stabilize_walk(pitch, roll) - - walk.bez.motor_control.set_motor() - walk.world.step() + target_goal = [1, 0, 0, 10, 2] + print("STARTING WALK") - t = walk.foot_step_planner.step(t) + for i in range(10000): + walk.walk(target_goal, display_metrics=False) + if i % 1000 == 0: + walk.reset_walk() def test_bez1_ready(self): self.world = PybulletWorld( From 6edb90e6aeda32cc14dcc8d47e7c6be24255e758 Mon Sep 17 00:00:00 2001 From: manx52 Date: Fri, 6 Dec 2024 19:27:36 -0500 Subject: [PATCH 03/24] added camera joint to bez1 and new robot. created a new class for better handeling of the motors based on str or int inputs, dont know if i fully like it. fixed camera image from pybullet, had to use the motors for rpy of the camera because the angles get weird. added a global and local ball ground truth. modified everywhere that interacted with motorcontrol. Got a ros version to work. swapped out pybullet setup with world and bez that control uses. no more duplicates. finished unit test for going to ball location based on yolov5. --- .idea/workspace.xml | 183 +++++++++++------- .../model/model_ros/bez_ros.py | 9 +- .../model/model_ros/motor_control_ros.py | 14 +- .../soccer_pycontrol/model/motor_control.py | 101 +++++++--- .../src/soccer_pycontrol/model/sensors.py | 22 ++- .../soccer_pycontrol/walk_engine/navigator.py | 19 +- .../walk_engine_ros/navigator_ros.py | 11 ++ .../soccer_pycontrol/test/test_placo.py | 74 ++++++- .../soccer_pycontrol/test/test_walk_ros.py | 4 +- .../src/soccer_trajectories/pybullet_setup.py | 87 --------- .../trajectory_manager_sim.py | 22 ++- .../test/test_trajectory.py | 4 +- .../assembly_description/urdf/assembly.urdf | 15 ++ .../bez1_description/urdf/bez1.urdf | 15 ++ .../camera/camera_calculations.py | 2 +- .../src/soccer_strategy/behavior_executive.py | 7 +- 16 files changed, 348 insertions(+), 241 deletions(-) delete mode 100644 soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py diff --git a/.idea/workspace.xml b/.idea/workspace.xml index bbf601937..59edea60f 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -8,13 +8,21 @@ - + + + - - - + + + + + + + + +