Skip to content

Commit

Permalink
Fix steering controllers library kinematics (#1150) (#1195)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mergify
mergify[bot] authored Jul 6, 2024
1 parent 485c561 commit 73b20e5
Show file tree
Hide file tree
Showing 7 changed files with 197 additions and 50 deletions.
6 changes: 5 additions & 1 deletion ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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);
Expand All @@ -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(
Expand Down
10 changes: 5 additions & 5 deletions bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -237,22 +237,22 @@ 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(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
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);
}

Expand Down
17 changes: 16 additions & 1 deletion steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_

#include <cmath>
#include <tuple>
#include <vector>

Expand All @@ -31,6 +32,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)
Expand Down Expand Up @@ -183,10 +187,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<double>, std::vector<double>> get_commands(
const double v_bx, const double omega_bz);
const double v_bx, const double omega_bz, const bool open_loop = true);

/**
* \brief Reset poses, heading, and accumulators
Expand Down Expand Up @@ -225,6 +230,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_linear_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
*/
Expand Down
2 changes: 1 addition & 1 deletion steering_controllers_library/src/steering_controllers_library.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -450,7 +450,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++)
Expand Down
82 changes: 60 additions & 22 deletions steering_controllers_library/src/steering_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <cmath>
#include <iostream>
#include <limits>

namespace steering_odometry
{
Expand Down Expand Up @@ -128,13 +129,26 @@ bool SteeringOdometry::update_from_velocity(
return update_odometry(linear_velocity, angular_velocity, dt);
}

double SteeringOdometry::get_linear_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_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_;

Expand All @@ -145,10 +159,18 @@ 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;
const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_;
// 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_linear_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);
}
Expand Down Expand Up @@ -181,30 +203,41 @@ 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<float>::epsilon())
{
return 0;
// avoid division by zero
return 0.;
}
return std::atan(omega_bz * wheelbase_ / v_bx);
}

std::tuple<std::vector<double>, std::vector<double>> 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_;

if (config_type_ == BICYCLE_CONFIG)
{
Expand All @@ -216,32 +249,37 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
{
std::vector<double> traction_commands;
std::vector<double> steering_commands;
if (fabs(steer_pos_) < 1e-6)
// double-traction axle
if (is_close_to_zero(phi_IK))
{
// 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);
}
else if (config_type_ == ACKERMANN_CONFIG)
{
std::vector<double> traction_commands;
std::vector<double> steering_commands;
if (fabs(steer_pos_) < 1e-6)
if (is_close_to_zero(phi_IK))
{
// 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};
Expand Down Expand Up @@ -279,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;
}

Expand All @@ -289,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);
Expand All @@ -300,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));
}
}

Expand Down
Loading

0 comments on commit 73b20e5

Please sign in to comment.