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 b1ac3b931..74fb9b63b 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 @@ -28,6 +28,7 @@ def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool = self.trajectory = None self.tasks = None self.solver = None + self.footstep_polygons = [] if self.debug: # Starting Meshcat viewer self.viz = robot_viz(self.robot) @@ -211,23 +212,24 @@ def step(self, t: float): def update_viz(self, supports: List[placo.Supports], trajectory: placo.WalkTrajectory): if self.debug: + self.footstep_polygons = [] # Drawing footsteps footsteps_viz(supports) # TODO look for step vis # TODO convert for ros - # steps = len(supports) - # T = np.eye(4) - # k = 0 - # for footstep in supports: - # k += 1 - # polygon = [(T @ [*xy, 0, 1])[:3] for xy in footstep.support_polygon()] - # polygon = np.array([*polygon, polygon[-1]]) - # - # polygon[:, 1] -= self.init_foot[1, 3]/2 - # p.addUserDebugLine(polygon[0], polygon[1], [1, 0,0], lifeTime=1) - # p.addUserDebugLine(polygon[1], polygon[2], [0, 1, 0], lifeTime=1) - # p.addUserDebugLine(polygon[2], polygon[3], [0, 0, 1], lifeTime=1) - # p.addUserDebugLine(polygon[0], polygon[3], [0, 1, 1], lifeTime=1) - # print("fsd") + steps = len(supports) + T = np.eye(4) + k = 0 + for footstep in supports: + k += 1 + polygon = [(T @ [*xy, 0, 1])[:3] for xy in footstep.support_polygon()] + polygon = np.array([*polygon, polygon[-1]]) + polygon[:, 1] -= self.init_foot[1, 3]/2 + self.footstep_polygons.append(polygon) + # p.addUserDebugLine(polygon[0], polygon[1], [1, 0, 0], lifeTime=1) + # p.addUserDebugLine(polygon[1], polygon[2], [0, 1, 0], lifeTime=1) + # p.addUserDebugLine(polygon[2], polygon[3], [0, 0, 1], lifeTime=1) + # p.addUserDebugLine(polygon[0], polygon[3], [0, 1, 1], lifeTime=1) + print("fsd") # Drawing planned CoM trajectory on the ground coms = [[*trajectory.get_p_world_CoM(t)[:2], 0.0] for t in np.linspace(trajectory.t_start, trajectory.t_end, 100)] line_viz("CoM_trajectory", np.array(coms), 0xFFAA00) 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 8f4cc8fe2..3d24028c9 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,7 +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): + def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = False): self.world = world self.bez = bez self.imu_feedback_enabled = imu_feedback_enabled diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py index e41bc7a95..7db47d59c 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py @@ -1,5 +1,8 @@ import rospy -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, Point +from std_msgs.msg import ColorRGBA +# from pybullet_examples.getClosestPoints import colorRGB +from visualization_msgs.msg import Marker, MarkerArray from soccer_pycontrol.model.model_ros.bez_ros import BezROS from soccer_pycontrol.walk_engine.foot_step_planner import FootStepPlanner from soccer_pycontrol.walk_engine.navigator import Navigator @@ -9,8 +12,9 @@ class NavigatorRos(Navigator): - def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False): + def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False, record_walking_metrics: bool = False): self.imu_feedback_enabled = imu_feedback_enabled + self.record_walking_metrics = record_walking_metrics self.bez = bez self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, rospy.get_time) @@ -44,6 +48,7 @@ def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False): self.error_tol = 0.05 # in m TODO add as a param and in the ros version self.position_subscriber = rospy.Subscriber(self.bez.ns + "goal", PoseStamped, self.goal_callback) + self.polygon_publisher = rospy.Publisher(self.bez.ns + "polygons", MarkerArray, queue_size=1) # set queue_size to 1, look at marker self.goal = PoseStamped() def goal_callback(self, pose: PoseStamped) -> None: @@ -61,3 +66,34 @@ def goal_callback(self, pose: PoseStamped) -> None: def wait(self, steps: int): for i in range(steps): rospy.sleep(self.foot_step_planner.DT) + + def walk_loop(self, t): + t= super(NavigatorRos, self).walk_loop(t) + polygons = MarkerArray() + for i, polygon in enumerate(self.foot_step_planner.footstep_polygons): + marker = Marker() + marker.header.frame_id = "map" # or map + marker.header.stamp = rospy.Time.now() + marker.type = marker.LINE_STRIP + marker.id = i + marker.action = marker.ADD + marker.scale.x = 0.01 + # marker.color.b = 1.0 + marker.color.a = 1.0 + r = ColorRGBA(1, 0, 0, 1) + g = ColorRGBA(0, 1, 0, 1) + b = ColorRGBA(0, 0, 1, 1) + p1 = Point(polygon[0][0], polygon[0][1], polygon[0][2]) + p2 = Point(polygon[1][0], polygon[1][1], polygon[1][2]) + p3 = Point(polygon[2][0], polygon[2][1], polygon[2][2]) + p4 = Point(polygon[3][0], polygon[3][1], polygon[3][2]) + p5 = Point(polygon[4][0], polygon[4][1], polygon[4][2]) + + marker.points.extend([p1,p2,p2,p3,p3,p4,p4,p5,p5,p1]) + marker.colors.extend([r,r,g,g,b,b,g,g,g,g]) + + polygons.markers.append(marker) + + self.polygon_publisher.publish(polygons) + return t +