From 8b20c3b3f6db22e8f127d3094f3b2028ef93d4e1 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Mon, 11 Nov 2024 21:36:02 +0100 Subject: [PATCH] Use foot pressure phase reset and PIDs in sim --- .../config/walking_wolfgang_simulator.yaml | 10 +++++----- .../bitbots_webots_sim/webots_robot_controller.py | 8 ++++++++ 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml index 4c38e7350..843c69281 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml @@ -75,23 +75,23 @@ walking: phase_reset: min_phase: 0.90 foot_pressure: - active: False + active: True ground_min_pressure: 1.5 effort: active: False joint_min_effort: 30.0 imu: - active: True + active: False y_acceleration_threshold: 1.4 trunk_pid: pitch: - p: 0.0 + p: 0.0035 i: 0.0 - d: 0.0 + d: 0.004 i_clamp_min: 0.0 i_clamp_max: 0.0 - antiwindup: False + antiwindup: false roll: p: 0.0 i: 0.0 diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py index 0ba93ed38..925fb1db3 100644 --- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py +++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py @@ -645,6 +645,12 @@ def __init__( self.pub_pres_left = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_left/raw", 1) self.pub_pres_right = self.ros_node.create_publisher(FootPressure, base_ns + "foot_pressure_right/raw", 1) + self.pub_pres_left_filtered = self.ros_node.create_publisher( + FootPressure, base_ns + "foot_pressure_left/filtered", 1 + ) + self.pub_pres_right_filtered = self.ros_node.create_publisher( + FootPressure, base_ns + "foot_pressure_right/filtered", 1 + ) self.cop_l_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_l", 1) self.cop_r_pub_ = self.ros_node.create_publisher(PointStamped, base_ns + "cop_r", 1) self.ros_node.create_subscription(JointCommand, base_ns + "DynamixelController/command", self.command_cb, 1) @@ -1009,6 +1015,8 @@ def get_pressure_message(self): def publish_pressure(self): left, right, cop_l, cop_r = self.get_pressure_message() self.pub_pres_left.publish(left) + self.pub_pres_left_filtered.publish(left) self.pub_pres_right.publish(right) + self.pub_pres_right_filtered.publish(right) self.cop_l_pub_.publish(cop_l) self.cop_r_pub_.publish(cop_r)