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 7b243e795c..41a4803237 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -151,6 +151,18 @@ class SteeringOdometry */ unsigned int get_odometry_type() const { return static_cast(config_type_); } + /** + * \brief Set tricycle config + * \param number of traction wheels + */ + void set_tricycle_config(const size_t nr_traction_wheels); + + /** + * \brief Get tricycle config + * \return number of traction wheels + */ + size_t get_tricycle_config() const { return tricycle_nr_traction_wheels_; } + /** * \brief heading getter * \return heading [rad] @@ -293,6 +305,7 @@ class SteeringOdometry /// Configuration type used for the forward kinematics int config_type_ = -1; + size_t tricycle_nr_traction_wheels_ = 1; /// Previous wheel position/state [rad]: double traction_wheel_old_pos_; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f34b3a78e0..c4eb56005c 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -174,13 +174,21 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) { - if (params_.traction_joints_names.size() != 2) + switch (params_.traction_joints_names.size()) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Tricycle configuration requires exactly two traction joints, but %zu were provided", - params_.traction_joints_names.size()); - return controller_interface::CallbackReturn::ERROR; + case 1: + odometry_.set_tricycle_config(1); + set_interface_numbers(2, 2, nr_ref_itfs_); + break; + case 2: + odometry_.set_tricycle_config(2); + break; + default: + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires exactly two traction joints, but %zu were provided", + params_.traction_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; } } else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) @@ -248,13 +256,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) { - if (params_.traction_joints_state_names.size() != 2) + if (params_.traction_joints_state_names.size() != odometry_.get_tricycle_config()) { RCLCPP_ERROR( get_node()->get_logger(), - "Tricycle configuration requires exactly two traction joints, but %zu state interface " + "Current tricycle configuration requires exactly %zu traction joints, but %zu state " + "interface " "names were provided", - params_.traction_joints_state_names.size()); + odometry_.get_tricycle_config(), params_.traction_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; } } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 5f480ca03d..76d8758bda 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -224,6 +224,11 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) config_type_ = static_cast(type); } +void SteeringOdometry::set_tricycle_config(const size_t nr_traction_wheels) +{ + tricycle_nr_traction_wheels_ = nr_traction_wheels; +} + double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { // phi can be nan if both v_bx and omega_bz are zero @@ -238,21 +243,19 @@ std::tuple, std::vector> SteeringOdometry::get_comma // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; -#if 0 - if (v_bx == 0 && omega_bz != 0) + // calculate steering angle + if ( + config_type_ == TRICYCLE_CONFIG && get_tricycle_config() == 1 && is_close_to_zero(v_bx) && + !is_close_to_zero(omega_bz)) { - // TODO(anyone) this would be only possible if traction is on the steering axis + // turning on the spot phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - // 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 + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); } -#endif - // steering angle - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + // steering angle for inverse kinematics if (open_loop) { phi_IK = phi; @@ -293,18 +296,35 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - // double-traction axle - if (is_close_to_zero(phi_IK)) + + if (get_tricycle_config() == 1) { - // avoid division by zero - traction_commands = {Ws, Ws}; + // if traction is on the steering axis + if (is_close_to_zero(v_bx) && !is_close_to_zero(omega_bz)) + { + Ws = abs(omega_bz) * wheel_base_ / wheel_radius_; + } + else + { + Ws /= std::cos(phi_IK); + } + traction_commands = {Ws}; } else { - const double turning_radius = wheel_base_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; - traction_commands = {Wr, Wl}; + // double-traction axle + if (is_close_to_zero(phi_IK)) + { + // avoid division by zero + traction_commands = {Ws, Ws}; + } + else + { + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } } // simple steering steering_commands = {phi}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index a211ac8bf4..c1c06b419c 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -287,13 +287,34 @@ TEST(TestSteeringOdometry, tricycle_IK_linear) steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_tricycle_config(2); + 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); + ASSERT_THAT(cmd0.size(), 2); + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0.0); auto cmd1 = std::get<1>(cmd); // steer - EXPECT_EQ(cmd1[0], 0); // no steering + ASSERT_THAT(cmd1.size(), 1); + EXPECT_EQ(cmd1[0], 0.0); // no steering +} + +TEST(TestSteeringOdometry, tricycle_single_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_tricycle_config(1); + + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + ASSERT_THAT(cmd0.size(), 1); + EXPECT_GT(cmd0[0], 0.0); // linear + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_EQ(cmd1[0], 0.0); // no steering } TEST(TestSteeringOdometry, tricycle_IK_left) @@ -301,6 +322,8 @@ 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.set_tricycle_config(2); + 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 @@ -315,6 +338,8 @@ 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.set_tricycle_config(2); + 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 @@ -329,6 +354,7 @@ 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.set_tricycle_config(2); { odom.update_from_position(0., -0.785, 1.); // already steered @@ -372,6 +398,8 @@ TEST(TestSteeringOdometry, tricycle_odometry) steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_tricycle_config(2); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2ede52e1b6..9f50b6ee75 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,6 +5,10 @@ tricycle_controller ===================== +.. note:: + + This controller is deprecated and will be removed in a future release. Please use the :ref:`tricycle_steering_controller_userdoc` instead. + Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. Input for control are robot base_link twist commands which are translated to traction and steering diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 6d32b8265a..70bc2a23b6 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -52,6 +52,9 @@ TricycleController::TricycleController() : controller_interface::ControllerInter CallbackReturn TricycleController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `TricycleController` is replaced by 'TricycleSteeringController'"); try { // Create the parameter listener and get the parameters diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst index 618d7f04a7..f5f3101f63 100644 --- a/tricycle_steering_controller/doc/userdoc.rst +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -5,9 +5,12 @@ tricycle_steering_controller ============================= -This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. +This controller implements the kinematics with two axes and three wheels. + +Two possible configurations are supported: +1. Two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. +2. A single traction wheel, which is also steerable: The controller expects a single command joint for traction and one commanding joint for steering. -The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 2c919cbfd8..e6868af4b6 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -72,6 +72,8 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_track_width); odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + // note: might get overwritten in the base class, because we don't have access to the number of + // joints here set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful");