Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wip][STEERING]: Add swerve drive #1148

Closed
wants to merge 2 commits into from
Closed
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
[STEERING]: Add swerve drive
Signed-off-by: Quique Llorente <ellorent@redhat.com>
  • Loading branch information
qinqon committed Jun 8, 2024
commit 5b2ba7ff57c616f5aa5d97a63dffb51d10c12964
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
35 changes: 35 additions & 0 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
71 changes: 71 additions & 0 deletions steering_controllers_library/test/test_steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}


194 changes: 194 additions & 0 deletions swerve_drive_controller/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -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)
------------------
Loading
Loading