Sim footsensor phase reset and PIDs (#620)

bit-bots · Nov 13, 2024 · 24c7a81 · 24c7a81
2 parents d40ab0c + 8b20c3b
commit 24c7a81
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 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
@@ -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
@@ -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)