[STEERING]: Add swerve drive

Signed-off-by: Quique Llorente <[email protected]>
ros-controls · Jun 8, 2024 · 5b2ba7f · 5b2ba7f
1 parent c83dce3
commit 5b2ba7f
Show file tree

Hide file tree

Showing 18 changed files with 1,684 additions and 0 deletions.
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/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)

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
@@ -258,6 +258,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 = {phi, phi, phi, phi};
+    }
+    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(phi);
+      double denominator_first_member = 2 * instantaneous_center * std::cos(phi);
+      double denominator_second_member = wheel_track_ * std::sin(phi);
+
+      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::sin(alpha_r);
+      double radius_l = instantaneous_center / std::sin(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");

diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp
@@ -109,6 +109,63 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right)
   EXPECT_LT(cmd1[0], 0);
 }
 
+TEST(TestSteeringOdometry, swerve_back_kin_linear)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::SWERVE_CONFIG);
+  odom.update_open_loop(1., 0., 1.);
+  auto cmd = odom.get_commands(1., 0.);
+  auto cmd0 = std::get<0>(cmd);  // vel
+  EXPECT_EQ(cmd0[0], cmd0[1]);   // linear
+  EXPECT_EQ(cmd0[0], cmd0[2]);   // linear
+  EXPECT_EQ(cmd0[0], cmd0[3]);   // linear
+  EXPECT_GT(cmd0[0], 0);
+  auto cmd1 = std::get<1>(cmd);  // steer
+  EXPECT_EQ(cmd1[0], cmd1[1]);   // no steering
+  EXPECT_EQ(cmd1[0], cmd1[2]);   // no steering
+  EXPECT_EQ(cmd1[0], cmd1[3]);   // no steering
+  EXPECT_EQ(cmd1[0], 0);
+}
+
+TEST(TestSteeringOdometry, swerve_back_kin_left)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::SWERVE_CONFIG);
+  odom.update_from_position(0., 0.2, 1.);  // assume already turn
+  auto cmd = odom.get_commands(1., 0.1);
+  auto cmd0 = std::get<0>(cmd);  // vel
+  EXPECT_GT(cmd0[0], cmd0[1]);   // front right (outer) > front left (inner)
+  EXPECT_GT(cmd0[0], 0);
+  EXPECT_EQ(cmd0[2], cmd0[0]);   // rear right == front right
+  EXPECT_EQ(cmd0[3], cmd0[1]);   // rear left  == front left
+  auto cmd1 = std::get<1>(cmd);  // steer
+  EXPECT_LT(cmd1[0], cmd1[1]);   // front right (outer) < front left (inner)
+  EXPECT_GT(cmd1[0], 0);
+  EXPECT_EQ(cmd1[2], -cmd1[0]);   // rear right == - front right
+  EXPECT_EQ(cmd1[3], -cmd1[1]);   // rear left  == - front left
+}
+
+TEST(TestSteeringOdometry, swerve_back_kin_right)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::SWERVE_CONFIG);
+  odom.update_from_position(0., -0.2, 1.);  // assume already turn
+  auto cmd = odom.get_commands(1., -0.1);
+  auto cmd0 = std::get<0>(cmd);  // vel
+  EXPECT_LT(cmd0[0], cmd0[1]);   // front right (inner) < front left (outer)
+  EXPECT_GT(cmd0[0], 0);
+  EXPECT_EQ(cmd0[2], cmd0[0]);   // rear right == front right
+  EXPECT_EQ(cmd0[3], cmd0[1]);   // rear left  == front left
+  auto cmd1 = std::get<1>(cmd);  // steer
+  EXPECT_LT(cmd1[0], cmd1[1]);   // front right (outer) < front left (inner)
+  EXPECT_LT(cmd1[0], 0);
+  EXPECT_EQ(cmd1[2], -cmd1[0]);   // rear right == - front right
+  EXPECT_EQ(cmd1[3], -cmd1[1]);   // rear left  == - front left
+}
+
 TEST(TestSteeringOdometry, bicycle_odometry)
 {
   steering_odometry::SteeringOdometry odom(1);
@@ -144,3 +201,17 @@ TEST(TestSteeringOdometry, ackermann_odometry)
   EXPECT_NEAR(odom.get_x(), .1, 1e-3);
   EXPECT_NEAR(odom.get_heading(), .01, 1e-3);
 }
+
+TEST(TestSteeringOdometry, swerve_odometry)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 1., 1.);
+  odom.set_odometry_type(steering_odometry::SWERVE_CONFIG);
+  ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1));
+  EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3);
+  EXPECT_NEAR(odom.get_angular(), .1, 1e-3);
+  EXPECT_NEAR(odom.get_x(), .1, 1e-3);
+  EXPECT_NEAR(odom.get_heading(), .01, 1e-3);
+}
+
+
diff --git a/swerve_drive_controller/CHANGELOG.rst b/swerve_drive_controller/CHANGELOG.rst
@@ -0,0 +1,194 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package swerve_drive_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+3.23.0 (2024-04-30)
+-------------------
+
+3.22.0 (2024-02-12)
+-------------------
+* Add test_depend on `hardware_interface_testing` (backport `#1018 <https://github.com/ros-controls/ros2_controllers/issues/1018>`_) (`#1020 <https://github.com/ros-controls/ros2_controllers/issues/1020>`_)
+* Add tests for `interface_configuration_type` consistently (backport `#899 <https://github.com/ros-controls/ros2_controllers/issues/899>`_) (`#1007 <https://github.com/ros-controls/ros2_controllers/issues/1007>`_)
+* Contributors: mergify[bot]
+
+3.21.0 (2024-01-20)
+-------------------
+
+3.20.2 (2024-01-11)
+-------------------
+
+3.20.1 (2024-01-08)
+-------------------
+
+3.20.0 (2024-01-03)
+-------------------
+
+3.19.2 (2023-12-12)
+-------------------
+
+3.19.1 (2023-12-05)
+-------------------
+
+3.19.0 (2023-12-01)
+-------------------
+
+3.18.0 (2023-11-21)
+-------------------
+
+3.17.0 (2023-10-31)
+-------------------
+* Improve docs (`#785 <https://github.com/ros-controls/ros2_controllers/issues/785>`_)
+* Contributors: Christoph Fröhlich
+
+3.16.0 (2023-09-20)
+-------------------
+
+3.15.0 (2023-09-11)
+-------------------
+
+3.14.0 (2023-08-16)
+-------------------
+
+3.13.0 (2023-08-04)
+-------------------
+
+3.12.0 (2023-07-18)
+-------------------
+
+3.11.0 (2023-06-24)
+-------------------
+* Added -Wconversion flag and fix warnings (`#667 <https://github.com/ros-controls/ros2_controllers/issues/667>`_)
+* Let sphinx add parameter description to documentation (`#651 <https://github.com/ros-controls/ros2_controllers/issues/651>`_)
+* Contributors: Christoph Fröhlich, gwalck
+
+3.10.1 (2023-06-06)
+-------------------
+
+3.10.0 (2023-06-04)
+-------------------
+
+3.9.0 (2023-05-28)
+------------------
+* Fix sphinx for steering odometry library/controllers (`#626 <https://github.com/ros-controls/ros2_controllers/issues/626>`_)
+* Steering odometry library and controllers (`#484 <https://github.com/ros-controls/ros2_controllers/issues/484>`_)
+* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković
+
+3.8.0 (2023-05-14)
+------------------
+
+3.7.0 (2023-05-02)
+------------------
+
+3.6.0 (2023-04-29)
+------------------
+
+3.5.0 (2023-04-14)
+------------------
+
+3.4.0 (2023-04-02)
+------------------
+
+3.3.0 (2023-03-07)
+------------------
+
+3.2.0 (2023-02-10)
+------------------
+
+3.1.0 (2023-01-26)
+------------------
+
+3.0.0 (2023-01-19)
+------------------
+
+2.15.0 (2022-12-06)
+-------------------
+
+2.14.0 (2022-11-18)
+-------------------
+
+2.13.0 (2022-10-05)
+-------------------
+
+2.12.0 (2022-09-01)
+-------------------
+
+2.11.0 (2022-08-04)
+-------------------
+
+2.10.0 (2022-08-01)
+-------------------
+
+2.9.0 (2022-07-14)
+------------------
+
+2.8.0 (2022-07-09)
+------------------
+
+2.7.0 (2022-07-03)
+------------------
+
+2.6.0 (2022-06-18)
+------------------
+
+2.5.0 (2022-05-13)
+------------------
+
+2.4.0 (2022-04-29)
+------------------
+
+2.3.0 (2022-04-21)
+------------------
+
+2.2.0 (2022-03-25)
+------------------
+
+2.1.0 (2022-02-23)
+------------------
+
+2.0.1 (2022-02-01)
+------------------
+
+2.0.0 (2022-01-28)
+------------------
+
+1.3.0 (2022-01-11)
+------------------
+
+1.2.0 (2021-12-29)
+------------------
+
+1.1.0 (2021-10-25)
+------------------
+
+1.0.0 (2021-09-29)
+------------------
+
+0.5.0 (2021-08-30)
+------------------
+
+0.4.1 (2021-07-08)
+------------------
+
+0.4.0 (2021-06-28)
+------------------
+
+0.3.1 (2021-05-23)
+------------------
+
+0.3.0 (2021-05-21)
+------------------
+
+0.2.1 (2021-05-03)
+------------------
+
+0.2.0 (2021-02-06)
+------------------
+
+0.1.2 (2021-01-07)
+------------------
+
+0.1.1 (2021-01-06)
+------------------
+
+0.1.0 (2020-12-23)
+------------------