give useful simulated imu values to use in pywrapper for webot

bit-bots · Jan 15, 2025 · 3f3ba52 · 3f3ba52
1 parent ee9b43e
commit 3f3ba52
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 3 deletions.
diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp
@@ -26,9 +26,13 @@ void WalkStabilizer::reset() {
 WalkRequest WalkStabilizer::adjust_step_length(WalkRequest request, const double imu_roll, const double imu_pitch,
                                                double pitch_threshold, double roll_threshold,
                                                const rclcpp::Duration& dt, walking::Params config_) {
+  pid_step_length_adjustment_pitch_.setGains(config_.node.step_length.pitch.p, 0.0, config_.node.step_length.pitch.d,
+                                             0.0, 0.0, false);
   double adjustment_pitch = pid_step_length_adjustment_pitch_.computeCommand(imu_pitch, dt);
+  RCLCPP_WARN(node_->get_logger(), "imu pitch: %f,", imu_pitch);
+  RCLCPP_WARN(node_->get_logger(), "pitch adjust: %f,", adjustment_pitch);
   double adjustment_roll = pid_step_length_adjustment_roll_.computeCommand(imu_roll, dt);
-  RCLCPP_WARN(node_->get_logger(), "Adjustment pitch, roll: %f, %f", adjustment_pitch, adjustment_roll);
+  // RCLCPP_WARN(node_->get_logger(), "Adjustment pitch, roll: %f, %f", adjustment_pitch, adjustment_roll);
   RCLCPP_WARN(node_->get_logger(), "p: %f,", config_.node.step_length.pitch.p);
 
   // adapt step length values based on PID controllers

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
@@ -605,11 +605,15 @@ def __init__(
         self.accel.enable(self.timestep)
         self.gyro = self.robot_node.getDevice(gyro_name)
         self.gyro.enable(self.timestep)
+        self.inertial = self.robot_node.getDevice("imu inertial")
+        self.inertial.enable(self.timestep)
         if self.is_wolfgang:
             self.accel_head = self.robot_node.getDevice("imu_head accelerometer")
             self.accel_head.enable(self.timestep)
             self.gyro_head = self.robot_node.getDevice("imu_head gyro")
             self.gyro_head.enable(self.timestep)
+            self.inertial_head = self.robot_node.getDevice("imu_head inertial")
+            self.inertial_head.enable(self.timestep)
         self.camera = self.robot_node.getDevice(camera_name)
         self.camera_counter = 0
         if self.camera_active:
@@ -790,7 +794,7 @@ def get_joint_state_msg(self):
     def publish_joint_states(self):
         self.pub_js.publish(self.get_joint_state_msg())
 
-    def get_imu_msg(self, head=False):
+    def get_imu_msg(self, head=False, orientation=False):
         msg = Imu()
         msg.header.stamp = Time(seconds=int(self.time), nanoseconds=self.time % 1 * 1e9).to_msg()
         if head:
@@ -828,6 +832,16 @@ def get_imu_msg(self, head=False):
                 msg.angular_velocity.z = gyro_vels[2]
             else:
                 msg.angular_velocity.z = 0.0
+
+        if orientation:
+            if head:
+                inertial_quat = self.inertial_head.getQuaternion()
+            else:
+                inertial_quat = self.inertial.getQuaternion()
+            msg.orientation.w = inertial_quat[3]
+            msg.orientation.x = inertial_quat[0]
+            msg.orientation.y = inertial_quat[1]
+            msg.orientation.z = inertial_quat[2]
         return msg
 
     def publish_imu(self):

diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py
@@ -245,7 +245,7 @@ def robot_pose_callback(self, request=None, response=None):
 
     def simulator_push(self, request=None, response=None):
         self.robot_nodes[request.robot].addForce([request.force.x, request.force.y, request.force.z], request.relative)
-        return response or Empty.Response()
+        return response or SimulatorPush.Response()
 
     def reset_ball(self, request=None, response=None):
         self.ball.getField("translation").setSFVec3f([0, 0, 0.0772])

diff --git a/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
@@ -355,6 +355,9 @@ PROTO Wolfgang [
         translation -0.036975 -0.047461 0.224000
         rotation 0.000000 1.000000 0.000000 0.000000
         children [
+          InertialUnit {
+            name "imu inertial"
+          }
           Accelerometer {
             name "imu accelerometer"
             lookupTable [
@@ -4522,6 +4525,9 @@ PROTO Wolfgang [
                     translation -0.025450 0.053500 -0.023500
                     rotation -1.000000 -0.000000 -0.000000 1.570800
                     children [
+                      InertialUnit {
+                        name "imu_head inertial"
+                      }
                       Accelerometer {
                         name "imu_head accelerometer"
                         lookupTable [