From 4a9367629fee416591b39133b2ae49660b55b544 Mon Sep 17 00:00:00 2001 From: Alexander Lampalzer Date: Wed, 8 Jun 2022 00:32:19 +0200 Subject: [PATCH] Improvements in odometry kinematics --- server_v2/src/OdometryController.cpp | 32 ++++++++++++++++------------ 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 28419c5..2ee64fe 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -56,34 +56,38 @@ OdometryController::OdometryController() { last_position_left = current_position_left; last_position_right = current_position_right; - auto distance = (distance_left + distance_right) * 2.0; - auto theta = (distance_left - distance_right) / ROBOT_ARBOR_LENGTH_M; + // The section below implements differential drive kinematics. + // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin + // or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart + // and Illah R. Nourbakhsh + + // Forward and Rotational velocity have already been integrated. + auto dist_forward = (distance_right + distance_left) / 2.0; + auto dist_rot = (distance_right + distance_left) / ROBOT_ARBOR_LENGTH_M; + + auto x = dist_forward * -sin(dist_rot); + auto y = dist_forward * cos(dist_rot); + auto theta = dist_rot; auto new_x_position = current_odometry.get_x_position(); auto new_y_position = current_odometry.get_y_position(); auto new_angular_orientation = current_odometry.get_angular_orientation(); - if (distance != 0) { - auto current_orientation = current_odometry.get_angular_orientation(); - auto distance_x = cos(theta) * distance; - auto distance_y = -sin(theta) * distance; - - new_x_position = current_odometry.get_x_position() + - (cos(current_orientation) * distance_x - - sin(current_orientation) * distance_y); - new_y_position = current_odometry.get_y_position() + - (sin(current_orientation) * distance_x + - cos(current_orientation) * distance_y); + if (dist_forward != 0) { + new_x_position = cos(theta) * x - sin(theta) * y + current_odometry.get_x_position(); + new_y_position = sin(theta) * x + cos(theta) * y + current_odometry.get_y_position(); } + if (theta != 0) { new_angular_orientation = mathUtils::wrap_angle_to_pi(current_odometry.get_angular_orientation() + theta); } + current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation); spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", current_position_left, current_position_right, distance_left, distance_right, - distance, theta); + dist_forward, dist_rot); } } }