diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 654234c795..e1d14b268a 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -53,6 +53,7 @@ pid_controller steering_controllers_library ******************************** * Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). +* New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). gpio_controllers ************************ 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 e4a8ed5566..ad265751fa 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -186,10 +186,13 @@ class SteeringOdometry * \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 + * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * has been reached * \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 = true); + const double v_bx, const double omega_bz, const bool open_loop = true, + const bool reduce_wheel_speed_until_steering_reached = false); /** * \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 5402345931..7b646974ff 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -452,8 +452,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; - auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); + auto [traction_commands, steering_commands] = odometry_.get_commands( + last_linear_velocity_, last_angular_velocity_, params_.open_loop, + params_.reduce_wheel_speed_until_steering_reached); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index a9f7fa75fb..376642d4be 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -63,6 +63,13 @@ steering_controllers_library: read_only: false, } + reduce_wheel_speed_until_steering_reached: { + type: bool, + default_value: false, + description: "Reduce wheel speed until the steering angle has been reached.", + read_only: false, + } + velocity_rolling_window_size: { type: int, default_value: 10, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b35d5c26a9..3a97215ae7 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -218,7 +218,8 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome } std::tuple, std::vector> SteeringOdometry::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, + const bool reduce_wheel_speed_until_steering_reached) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -245,6 +246,29 @@ std::tuple, std::vector> SteeringOdometry::get_comma // wheel speed Ws = v_bx / wheel_radius_; + if (!open_loop && reduce_wheel_speed_until_steering_reached) + { + // Reduce wheel speed until the target angle has been reached + double phi_delta = abs(steer_pos_ - phi); + double scale; + const double min_phi_delta = M_PI / 6.; + if (phi_delta < min_phi_delta) + { + scale = 1; + } + else if (phi_delta >= 1.5608) + { + // cos(1.5608) = 0.01 + scale = 0.01 / cos(min_phi_delta); + } + else + { + // TODO(anyone): find the best function, e.g convex power functions + scale = cos(phi_delta) / cos(min_phi_delta); + } + Ws *= scale; + } + if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d93e29eca5..27ef11548a 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -116,13 +116,59 @@ TEST(TestSteeringOdometry, ackermann_IK_right) 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_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) EXPECT_LT(cmd1[0], 0); } +TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } +} + // ----------------- bicycle ----------------- TEST(TestSteeringOdometry, bicycle_IK_linear) @@ -164,6 +210,62 @@ TEST(TestSteeringOdometry, bicycle_IK_right) EXPECT_LT(cmd1[0], 0); // left steering } +TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited) +{ + 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.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_not_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered_limited; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + vel_cmd_not_steered_limited = std::get<0>(cmd); // vel + EXPECT_GT(vel_cmd_not_steered_limited[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < vel_cmd_not_steered_limited.size(); ++i) + { + EXPECT_LT(vel_cmd_not_steered_limited[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + // larger error -> check min of scale + odom.update_from_position(0., M_PI, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered_limited now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered_limited[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -214,12 +316,55 @@ TEST(TestSteeringOdometry, tricycle_IK_right) 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_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_IK_right_steering_limited) +{ + 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.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1);