compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Improvements in odometry kinematics

Browse source
This commit is contained in:
Alexander Lampalzer 2022-06-08 00:32:19 +02:00
parent 7fe160211f
commit 4a9367629f
1 changed files with 18 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

32
server_v2/src/OdometryController.cpp
View file

@ -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);
}
}
}