From aa34f02591a477e3222571f2a9d6c4f3a29a1de9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 01/10] Fix IK - in open-loop mode, zero steering angle was used for IK calculation - wrong wheelspeed was used (traction on steering wheel, instead of other axle was assumed) --- .../steering_odometry.hpp | 3 +- .../src/steering_controllers_library.cpp | 2 +- .../src/steering_odometry.cpp | 40 ++++++++++++++----- .../test/test_steering_odometry.cpp | 6 +-- 4 files changed, 35 insertions(+), 16 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 index 0104b011c7..4c9709a86f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -188,10 +188,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> get_commands( - const double v_bx, const double omega_bz); + const double v_bx, const double omega_bz, const bool open_loop); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5880000d17..8ef7927195 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -401,7 +401,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++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 32cdb69454..788b01d9ef 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace steering_odometry { @@ -181,30 +182,42 @@ 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::epsilon()) { - return 0; + // avoid division by zero + return 0.; } return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> 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_; + std::cout << "Ws: " << Ws << " phi: " << phi << " phi_IK: " << phi_IK << std::endl; if (config_type_ == BICYCLE_CONFIG) { @@ -216,17 +229,20 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (fabs(phi_IK) < 1e-6) { + // 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); } @@ -234,14 +250,16 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (fabs(phi_IK) < 1e-6) { + // 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}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..b9bd2afc3d 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -70,7 +70,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); - auto cmd = odom.get_commands(1., 0.); + auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -85,7 +85,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., 0.1); + auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -100,7 +100,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., -0.1); + auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); From 35f34d8004cf683fab3a451cf724db69a254bec4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 02/10] Fix controller tests and add notes --- .../test/test_ackermann_steering_controller.cpp | 6 +++++- .../test/test_bicycle_steering_controller.cpp | 10 +++++----- .../test/test_tricycle_steering_controller.cpp | 3 +++ 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..e4f0aae146 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/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( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..db7a9da67e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/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,7 +237,7 @@ 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( @@ -245,14 +245,14 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status 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); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..328f5e4d6a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -201,6 +201,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) 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); @@ -246,6 +247,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu 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); @@ -258,6 +260,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu 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[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( From 21afbc515b6d99c0159ba2b043801cdaee966f01 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 03/10] Improve tests for other steering types --- .../test/test_steering_odometry.cpp | 119 +++++++++++++++--- 1 file changed, 103 insertions(+), 16 deletions(-) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index b9bd2afc3d..d39130c02f 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_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); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,7 +78,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -79,7 +93,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -94,7 +108,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -109,6 +123,47 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -121,24 +176,56 @@ TEST(TestSteeringOdometry, bicycle_odometry) EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } -TEST(TestSteeringOdometry, tricycle_odometry) +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) { steering_odometry::SteeringOdometry odom(1); - odom.set_wheel_params(1., 1., 1.); + odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(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); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering } -TEST(TestSteeringOdometry, ackermann_odometry) +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(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); From 8809f6211dc9629d5760caa7e66169283320e967 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 04/10] Improve odometry of overdetermined measurements --- .../steering_odometry.hpp | 10 ++++++ .../src/steering_odometry.cpp | 31 ++++++++++++++++--- .../test/test_steering_odometry.cpp | 4 +-- 3 files changed, 38 insertions(+), 7 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 index 4c9709a86f..75e0a00147 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -231,6 +231,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_lin_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 */ diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 788b01d9ef..f006a49bce 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -129,13 +129,26 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } +double SteeringOdometry::get_lin_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_lin_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; @@ -146,9 +159,17 @@ 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; + // 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_lin_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); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d39130c02f..d93e29eca5 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -36,7 +36,7 @@ TEST(TestSteeringOdometry, ackermann_odometry) odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_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_linear(), 1.002, 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); @@ -226,7 +226,7 @@ TEST(TestSteeringOdometry, tricycle_odometry) odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_linear(), 1.002, 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); From 0209540ef1c24eca5ef3175b2fd9a71d9007689e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 05/10] Remove debug output --- steering_controllers_library/src/steering_odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index f006a49bce..9b97f51a74 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -238,7 +238,6 @@ std::tuple, std::vector> SteeringOdometry::get_comma } // wheel speed Ws = v_bx / wheel_radius_; - std::cout << "Ws: " << Ws << " phi: " << phi << " phi_IK: " << phi_IK << std::endl; if (config_type_ == BICYCLE_CONFIG) { From f7486dd3cd27af02ffb78f7224b18b67284ba90e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 8 Jun 2024 15:03:05 +0200 Subject: [PATCH 06/10] Use std:: namespace for trigonometric functions --- .../src/steering_odometry.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 9b97f51a74..9d40b00fc7 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -124,7 +124,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -150,7 +150,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_lin_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -317,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; } @@ -338,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)); } } From 8729f9e6cb7e4846e0bd241a4c171dab5dff0650 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Jun 2024 17:27:58 +0000 Subject: [PATCH 07/10] Use the full name --- .../include/steering_controllers_library/steering_odometry.hpp | 2 +- steering_controllers_library/src/steering_odometry.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 index 75e0a00147..8b37df384e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -237,7 +237,7 @@ class SteeringOdometry * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] */ - double get_lin_velocity_double_traction_axle( + double get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 9d40b00fc7..5de793f889 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -129,7 +129,7 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -double SteeringOdometry::get_lin_velocity_double_traction_axle( +double SteeringOdometry::get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos) { From 37b8f0a8b7f6cdd1d41dd4104f729ee66ff23540 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Jun 2024 17:46:13 +0000 Subject: [PATCH 08/10] Fix method name --- steering_controllers_library/src/steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index d27c2666f1..05850f7cf3 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -147,7 +147,7 @@ bool SteeringOdometry::update_from_velocity( const double steer_pos, const double dt) { steer_pos_ = steer_pos; - double linear_velocity = get_lin_velocity_double_traction_axle( + 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_; From 1504bc5835f38edc1a47e5adf33b8f08e4afe2cb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Jun 2024 20:23:33 +0000 Subject: [PATCH 09/10] Add helper function --- .../steering_controllers_library/steering_odometry.hpp | 4 ++++ steering_controllers_library/src/steering_odometry.cpp | 6 +++--- 2 files changed, 7 insertions(+), 3 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 index 8b37df384e..f668bac979 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/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 #include #include @@ -36,6 +37,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) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 05850f7cf3..f8b42771e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -250,7 +250,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma std::vector traction_commands; std::vector steering_commands; // double-traction axle - if (fabs(phi_IK) < 1e-6) + if (is_close_to_zero(phi_IK)) { // avoid division by zero traction_commands = {Ws, Ws}; @@ -270,7 +270,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(phi_IK) < 1e-6) + if (is_close_to_zero(phi_IK)) { // avoid division by zero traction_commands = {Ws, Ws}; @@ -327,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); From fe2ba4b0354805523cc42442909902011533f722 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 21:22:31 +0000 Subject: [PATCH 10/10] Add default value to avoid breaking API --- .../include/steering_controllers_library/steering_odometry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index f668bac979..dadc5730d4 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -196,7 +196,7 @@ class SteeringOdometry * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz, const bool open_loop); + const double v_bx, const double omega_bz, const bool open_loop = true); /** * \brief Reset poses, heading, and accumulators