Fix steering controllers library kinematics (#1150)

(cherry picked from commit df04492)

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
@@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-  EXPECT_NEAR(
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
+  EXPECT_NEAR(
     controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
   EXPECT_NEAR(
@@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
@@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
 
   subscribe_and_get_messages(msg);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
@@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
@@ -237,22 +237,22 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
   EXPECT_EQ(msg.linear_velocity_command[0], 1.1);
   EXPECT_EQ(msg.steering_angle_command[0], 2.2);
 
-  publish_commands();
+  publish_commands(0.1, 0.2);
   ASSERT_TRUE(controller_->wait_for_commands(executor));
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   subscribe_and_get_messages(msg);
 
-  EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD);
+  EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD);
 }
 

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -18,6 +18,7 @@
 #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
 #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
 
+#include <cmath>
 #include <tuple>
 #include <vector>
 
@@ -31,6 +32,9 @@ namespace steering_odometry
 const unsigned int BICYCLE_CONFIG = 0;
 const unsigned int TRICYCLE_CONFIG = 1;
 const unsigned int ACKERMANN_CONFIG = 2;
+
+inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; }
+
 /**
  * \brief The Odometry class handles odometry readings
  * (2D pose and velocity with related timestamp)
@@ -183,10 +187,11 @@ class SteeringOdometry
    * \brief Calculates inverse kinematics for the desired linear and angular velocities
    * \param v_bx     Desired linear velocity of the robot in x_b-axis direction
    * \param omega_bz Desired angular velocity of the robot around x_z-axis
+   * \param open_loop If false, the IK will be calculated using measured steering angle
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
-    const double v_bx, const double omega_bz);
+    const double v_bx, const double omega_bz, const bool open_loop = true);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -225,6 +230,16 @@ class SteeringOdometry
    */
   double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
+  /**
+   * \brief Calculates linear velocity of a robot with double traction axle
+   * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
+   * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
+   * \param steer_pos Steer wheel position [rad]
+   */
+  double get_linear_velocity_double_traction_axle(
+    const double right_traction_wheel_vel, const double left_traction_wheel_vel,
+    const double steer_pos);
+
   /**
    *  \brief Reset linear and angular accumulators
    */

steering_controllers_library/src/steering_controllers_library.cpp

@@ -450,7 +450,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     last_angular_velocity_ = reference_interfaces_[1];
 
     auto [traction_commands, steering_commands] =
-      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_);
+      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop);
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)

steering_controllers_library/src/steering_odometry.cpp

@@ -21,6 +21,7 @@
 
 #include <cmath>
 #include <iostream>
+#include <limits>
 
 namespace steering_odometry
 {
@@ -128,13 +129,26 @@ bool SteeringOdometry::update_from_velocity(
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
+double SteeringOdometry::get_linear_velocity_double_traction_axle(
+  const double right_traction_wheel_vel, const double left_traction_wheel_vel,
+  const double steer_pos)
+{
+  double turning_radius = wheelbase_ / std::tan(steer_pos);
+  // overdetermined, we take the average
+  double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius /
+                 (turning_radius + wheel_track_ * 0.5);
+  double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius /
+                 (turning_radius - wheel_track_ * 0.5);
+  return (vel_r + vel_l) * 0.5;
+}
+
 bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
-  double linear_velocity =
-    (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
   steer_pos_ = steer_pos;
+  double linear_velocity = get_linear_velocity_double_traction_axle(
+    right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
 
   const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_;
 
@@ -145,10 +159,18 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-  steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
-  double linear_velocity =
-    (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
-  const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_;
+  // overdetermined, we take the average
+  const double right_steer_pos_est = std::atan(
+    wheelbase_ * std::tan(right_steer_pos) /
+    (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos)));
+  const double left_steer_pos_est = std::atan(
+    wheelbase_ * std::tan(left_steer_pos) /
+    (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos)));
+  steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5;
+
+  double linear_velocity = get_linear_velocity_double_traction_axle(
+    right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
+  const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_;
 
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
@@ -181,30 +203,41 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_
 
 double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
 {
-  if (omega_bz == 0 || v_bx == 0)
+  if (fabs(v_bx) < std::numeric_limits<float>::epsilon())
   {
-    return 0;
+    // avoid division by zero
+    return 0.;
   }
   return std::atan(omega_bz * wheelbase_ / v_bx);
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  const double v_bx, const double omega_bz)
+  const double v_bx, const double omega_bz, const bool open_loop)
 {
   // desired wheel speed and steering angle of the middle of traction and steering axis
-  double Ws, phi;
+  double Ws, phi, phi_IK = steer_pos_;
 
+#if 0
   if (v_bx == 0 && omega_bz != 0)
   {
-    // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller
+    // TODO(anyone) this would be only possible if traction is on the steering axis
     phi = omega_bz > 0 ? M_PI_2 : -M_PI_2;
     Ws = abs(omega_bz) * wheelbase_ / wheel_radius_;
   }
   else
   {
-    phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
-    Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_));
+    // TODO(anyone) this would be valid only if traction is on the steering axis
+    Ws = v_bx / (wheel_radius_ * std::cos(phi_IK));  // using the measured steering angle
+  }
+#endif
+  // steering angle
+  phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+  if (open_loop)
+  {
+    phi_IK = phi;
   }
+  // wheel speed
+  Ws = v_bx / wheel_radius_;
 
   if (config_type_ == BICYCLE_CONFIG)
   {
@@ -216,32 +249,37 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   {
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
-    if (fabs(steer_pos_) < 1e-6)
+    // double-traction axle
+    if (is_close_to_zero(phi_IK))
     {
+      // avoid division by zero
       traction_commands = {Ws, Ws};
     }
     else
     {
-      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double turning_radius = wheelbase_ / std::tan(phi_IK);
       const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
       const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
     }
+    // simple steering
     steering_commands = {phi};
     return std::make_tuple(traction_commands, steering_commands);
   }
   else if (config_type_ == ACKERMANN_CONFIG)
   {
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
-    if (fabs(steer_pos_) < 1e-6)
+    if (is_close_to_zero(phi_IK))
     {
+      // avoid division by zero
       traction_commands = {Ws, Ws};
+      // shortcut, no steering
       steering_commands = {phi, phi};
     }
     else
     {
-      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double turning_radius = wheelbase_ / std::tan(phi_IK);
       const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
       const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
@@ -279,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2(
   const double theta_mid = heading_ + omega_bz * 0.5 * dt;
 
   // Use the intermediate values to update the state
-  x_ += v_bx * cos(theta_mid) * dt;
-  y_ += v_bx * sin(theta_mid) * dt;
+  x_ += v_bx * std::cos(theta_mid) * dt;
+  y_ += v_bx * std::sin(theta_mid) * dt;
   heading_ += omega_bz * dt;
 }
 
@@ -289,7 +327,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co
   const double delta_x_b = v_bx * dt;
   const double delta_theta = omega_bz * dt;
 
-  if (fabs(delta_theta) < 1e-6)
+  if (is_close_to_zero(delta_theta))
   {
     /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero):
     integrate_runge_kutta_2(v_bx, omega_bz, dt);
@@ -300,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co
     const double heading_old = heading_;
     const double R = delta_x_b / delta_theta;
     heading_ += delta_theta;
-    x_ += R * (sin(heading_) - sin(heading_old));
-    y_ += -R * (cos(heading_) - cos(heading_old));
+    x_ += R * (sin(heading_) - std::sin(heading_old));
+    y_ += -R * (cos(heading_) - std::cos(heading_old));
   }
 }