[STEERING]: Add swerve drive

Signed-off-by: Quique Llorente <[email protected]>

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -136,8 +136,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   double last_linear_velocity_ = 0.0;
   double last_angular_velocity_ = 0.0;
 
-  std::vector<std::string> rear_wheels_state_names_;
-  std::vector<std::string> front_wheels_state_names_;
+  std::vector<std::string> wheels_state_names_;
+  std::vector<std::string> steers_state_names_;
 
 private:
   // callback for topic interface

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -36,6 +36,7 @@ namespace steering_odometry
 const unsigned int BICYCLE_CONFIG = 0;
 const unsigned int TRICYCLE_CONFIG = 1;
 const unsigned int ACKERMANN_CONFIG = 2;
+const unsigned int SWERVE_CONFIG = 3;
 /**
  * \brief The Odometry class handles odometry readings
  * (2D pose and velocity with related timestamp)

steering_controllers_library/src/steering_controllers_library.cpp

@@ -89,22 +89,22 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
 
   configure_odometry();
 
-  if (!params_.rear_wheels_state_names.empty())
+  if (!params_.wheels_state_names.empty())
   {
-    rear_wheels_state_names_ = params_.rear_wheels_state_names;
+    wheels_state_names_ = params_.wheels_state_names;
   }
   else
   {
-    rear_wheels_state_names_ = params_.rear_wheels_names;
+    wheels_state_names_ = params_.wheels_names;
   }
 
-  if (!params_.front_wheels_state_names.empty())
+  if (!params_.steers_state_names.empty())
   {
-    front_wheels_state_names_ = params_.front_wheels_state_names;
+    steers_state_names_ = params_.steers_state_names;
   }
   else
   {
-    front_wheels_state_names_ = params_.front_wheels_names;
+    steers_state_names_ = params_.steers_names;
   }
 
   // topics QoS
@@ -286,34 +286,16 @@ SteeringControllersLibrary::command_interface_configuration() const
   controller_interface::InterfaceConfiguration command_interfaces_config;
   command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   command_interfaces_config.names.reserve(nr_cmd_itfs_);
-
-  if (params_.front_steering)
+  for (size_t i = 0; i < params_.wheels_names.size(); i++)
   {
-    for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
-    {
-      command_interfaces_config.names.push_back(
-        params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY);
-    }
-
-    for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
-    {
-      command_interfaces_config.names.push_back(
-        params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION);
-    }
+    command_interfaces_config.names.push_back(
+      params_.wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY);
   }
-  else
-  {
-    for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
-    {
-      command_interfaces_config.names.push_back(
-        params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY);
-    }
 
-    for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
-    {
-      command_interfaces_config.names.push_back(
-        params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION);
-    }
+  for (size_t i = 0; i < params_.steers_names.size(); i++)
+  {
+    command_interfaces_config.names.push_back(
+      params_.steers_names[i] + "/" + hardware_interface::HW_IF_POSITION);
   }
   return command_interfaces_config;
 }
@@ -328,33 +310,17 @@ SteeringControllersLibrary::state_interface_configuration() const
   const auto traction_wheels_feedback = params_.position_feedback
                                           ? hardware_interface::HW_IF_POSITION
                                           : hardware_interface::HW_IF_VELOCITY;
-  if (params_.front_steering)
+
+  for (size_t i = 0; i < wheels_state_names_.size(); i++)
   {
-    for (size_t i = 0; i < rear_wheels_state_names_.size(); i++)
-    {
-      state_interfaces_config.names.push_back(
-        rear_wheels_state_names_[i] + "/" + traction_wheels_feedback);
-    }
-
-    for (size_t i = 0; i < front_wheels_state_names_.size(); i++)
-    {
-      state_interfaces_config.names.push_back(
-        front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION);
-    }
+    state_interfaces_config.names.push_back(
+      wheels_state_names_[i] + "/" + traction_wheels_feedback);
   }
-  else
-  {
-    for (size_t i = 0; i < front_wheels_state_names_.size(); i++)
-    {
-      state_interfaces_config.names.push_back(
-        front_wheels_state_names_[i] + "/" + traction_wheels_feedback);
-    }
 
-    for (size_t i = 0; i < rear_wheels_state_names_.size(); i++)
-    {
-      state_interfaces_config.names.push_back(
-        rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION);
-    }
+  for (size_t i = 0; i < steers_state_names_.size(); i++)
+  {
+    state_interfaces_config.names.push_back(
+      steers_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION);
   }
 
   return state_interfaces_config;
@@ -451,30 +417,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
 
     auto [traction_commands, steering_commands] =
       odometry_.get_commands(last_linear_velocity_, last_angular_velocity_);
-    if (params_.front_steering)
+    for (size_t i = 0; i < params_.wheels_names.size(); i++)
     {
-      for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
-      {
-        command_interfaces_[i].set_value(traction_commands[i]);
-      }
-      for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
-      {
-        command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]);
-      }
+      command_interfaces_[i].set_value(traction_commands[i]);
     }
-    else
+    for (size_t i = 0; i < params_.steers_names.size(); i++)
     {
-      {
-        for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
-        {
-          command_interfaces_[i].set_value(traction_commands[i]);
-        }
-        for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
-        {
-          command_interfaces_[i + params_.front_wheels_names.size()].set_value(
-            steering_commands[i]);
-        }
-      }
+      command_interfaces_[i + params_.wheels_names.size()].set_value(steering_commands[i]);
     }
   }
 
@@ -517,14 +466,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     controller_state_publisher_->msg_.steer_positions.clear();
     controller_state_publisher_->msg_.steering_angle_command.clear();
 
-    auto number_of_traction_wheels = params_.rear_wheels_names.size();
-    auto number_of_steering_wheels = params_.front_wheels_names.size();
-
-    if (!params_.front_steering)
-    {
-      number_of_traction_wheels = params_.front_wheels_names.size();
-      number_of_steering_wheels = params_.rear_wheels_names.size();
-    }
+    auto number_of_traction_wheels = params_.wheels_names.size();
+    auto number_of_steering_wheels = params_.steers_names.size();
 
     for (size_t i = 0; i < number_of_traction_wheels; ++i)
     {

steering_controllers_library/src/steering_controllers_library.yaml

@@ -5,16 +5,9 @@ steering_controllers_library:
     description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.",
   }
 
-  front_steering: {
-    type: bool,
-    default_value: true,
-    description: "Is the steering on the front of the robot?",
-    read_only: true,
-  }
-
-  rear_wheels_names: {
+  wheels_names: {
     type: string_array,
-    description: "Names of rear wheel joints.",
+    description: "Names of wheel joints.",
     read_only: true,
     validation: {
       size_lt<>: [5],
@@ -23,9 +16,9 @@ steering_controllers_library:
     }
   }
 
-  front_wheels_names: {
+  steers_names: {
     type: string_array,
-    description: "Names of front wheel joints.",
+    description: "Names of steering joints.",
     read_only: true,
     validation: {
       size_lt<>: [5],
@@ -34,9 +27,9 @@ steering_controllers_library:
     }
   }
 
-  rear_wheels_state_names: {
+  wheels_state_names: {
     type: string_array,
-    description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.",
+    description: "(Optional) Names of wheel joints to read states from. If not set joint names from 'wheels_names' will be used.",
     default_value: [],
     read_only: true,
     validation: {
@@ -45,9 +38,9 @@ steering_controllers_library:
     }
   }
 
-  front_wheels_state_names: {
+  steers_state_names: {
     type: string_array,
-    description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.",
+    description: "(Optional) Names of wheel joints to read states from. If not set joint names from 'steer_names' will be used.",
     default_value: [],
     read_only: true,
     validation: {

steering_controllers_library/src/steering_odometry.cpp

@@ -276,6 +276,41 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     }
     return std::make_tuple(traction_commands, steering_commands);
   }
+  else if (config_type_ == SWERVE_CONFIG)
+  {
+    std::vector<double> traction_commands;
+    std::vector<double> steering_commands;
+    if (fabs(steer_pos_) < 1e-6)
+    {
+      traction_commands = {Ws, Ws, Ws, Ws};
+      steering_commands = {alpha, alpha, alpha, alpha};
+    }
+    else
+    {
+      //TODO: this is a simplifierd swerve drive were 
+      //      instantenous center is alwys in the middle and
+      //      kingping distance is zero.
+      double instantaneous_center = wheelbase_ * 0.5;
+      double turning_radius = instantaneous_center / std::tan(steer_pos_);
+
+      double numerator = 2 * instantaneous_center * std::sin(alpha);
+      double denominator_first_member = 2 * instantaneous_center * std::cos(alpha);
+      double denominator_second_member = wheel_track_ * std::sin(alpha);
+
+      double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
+      double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);
+      steering_commands = {alpha_r, alpha_l, -alpha_r, -alpha_l};
+
+      double radius_r = instantaneous_center / std::cos(alpha_r);
+      double radius_l = instantaneous_center / std::cos(alpha_l);
+
+      double Wr = Ws * (radius_r / turning_radius);
+      double Wl = Ws * (radius_l / turning_radius);
+      traction_commands = {Wr, Wl, Wr, Wl};
+
+    }
+    return std::make_tuple(traction_commands, steering_commands);
+  }
   else
   {
     throw std::runtime_error("Config not implemented");

steering_controllers_library/test/steering_controllers_library_params.yaml

@@ -7,8 +7,8 @@ test_steering_controllers_library:
     velocity_rolling_window_size: 10
     position_feedback: false
     use_stamped_vel: true
-    rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
-    front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
+    wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
+    steers_names: [front_right_steering_joint, front_left_steering_joint]
 
     wheelbase: 3.24644
     front_wheel_track: 2.12321

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -38,32 +38,32 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
   ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
   EXPECT_EQ(
     cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
-    rear_wheels_names_[0] + "/" + traction_interface_name_);
+    wheels_names_[0] + "/" + traction_interface_name_);
   EXPECT_EQ(
     cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
-    rear_wheels_names_[1] + "/" + traction_interface_name_);
+    wheels_names_[1] + "/" + traction_interface_name_);
   EXPECT_EQ(
     cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
-    front_wheels_names_[0] + "/" + steering_interface_name_);
+    steers_names_[0] + "/" + steering_interface_name_);
   EXPECT_EQ(
     cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
-    front_wheels_names_[1] + "/" + steering_interface_name_);
+    steers_names_[1] + "/" + steering_interface_name_);
   EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
   auto state_if_conf = controller_->state_interface_configuration();
   ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
   EXPECT_EQ(
     state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
-    controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_);
+    controller_->wheels_state_names_[0] + "/" + traction_interface_name_);
   EXPECT_EQ(
     state_if_conf.names[STATE_TRACTION_LEFT_WHEEL],
-    controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_);
+    controller_->wheels_state_names_[1] + "/" + traction_interface_name_);
   EXPECT_EQ(
     state_if_conf.names[STATE_STEER_RIGHT_WHEEL],
-    controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_);
+    controller_->steers_state_names_[0] + "/" + steering_interface_name_);
   EXPECT_EQ(
     state_if_conf.names[STATE_STEER_LEFT_WHEEL],
-    controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_);
+    controller_->steers_state_names_[1] + "/" + steering_interface_name_);
   EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
   // check ref itfsTIME