Isaac Sim Version
5.1.0
5.0.0
Detailed Description
This code looks wrong:
File source/extensions/isaacsim.robot.wheeled_robots/python/controllers/ackermann_controller.py
# the distance between the center of rotation and the center of movement for robot body
R = ((-1.0 if self.invert_steering else 1.0) * self.wheel_base) / np.tan(steering_angle)
# Equations were simplied from the ones shown in https://www.mathworks.com/help/vdynblks/ref/kinematicsteering.html
# compute the wheel angles by taking into account their offset from the center of the turning axle (where the bicycle model is centered), then computing the angles of each wheel relative to the turning point of the robot
# Assuming four wheel drive with forward steering
self.left_wheel_angle = np.arctan(self.wheel_base / (R - 0.5 * self.track_width))
self.right_wheel_angle = np.arctan(self.wheel_base / (R + 0.5 * self.track_width))
# distance of left and right wheels to middle of front axle
steering_joint_half_dist = self.track_width / 2.0
cy = np.fabs(R)
# Calculate distances based on drive type and steering angle
sign = 1.0 if steering_angle > 0 else -1.0 # 1 for positive, -1 for negative
if self.invert_steering:
# Rear wheel steering case
# finding the distance between each wheel and the ICR
self.wheel_dist_FL = cy - sign * steering_joint_half_dist
self.wheel_dist_FR = cy + sign * steering_joint_half_dist
self.wheel_dist_BL = np.sqrt(
np.square(cy - sign * steering_joint_half_dist) + np.square(self.wheel_base)
)
self.wheel_dist_BR = np.sqrt(
np.square(cy + sign * steering_joint_half_dist) + np.square(self.wheel_base)
)
else:
# Forward steering case
# finding the distance between each wheel and the ICR
self.wheel_dist_FL = np.sqrt(
np.square(cy - sign * steering_joint_half_dist) + np.square(self.wheel_base)
)
self.wheel_dist_FR = np.sqrt(
np.square(cy + sign * steering_joint_half_dist) + np.square(self.wheel_base)
)
self.wheel_dist_BL = cy - sign * steering_joint_half_dist
self.wheel_dist_BR = cy + sign * steering_joint_half_dist
# angular velocity of the robot body
body_ang_vel = forward_vel / cy
# angular velocity of each wheel
self.wheel_rotation_velocity_FL = body_ang_vel * (self.wheel_dist_FL / self.front_wheel_radius)
self.wheel_rotation_velocity_FR = body_ang_vel * (self.wheel_dist_FR / self.front_wheel_radius)
self.wheel_rotation_velocity_BL = body_ang_vel * (self.wheel_dist_BL / self.back_wheel_radius)
self.wheel_rotation_velocity_BR = body_ang_vel * (self.wheel_dist_BR / self.back_wheel_radius)
For the convention about the steering_angle in the forward-steering and rear-steering (invert_steering=True) case, there are four possible conventions:
a1: When car goes forward and turns left, steering_angle is positive
a2: When car goes forward and turns right, steering_angle is positive
b1. When car goes forward and the steering wheel rotates counter-clockwise viewed from above, steering_angle is positive
b2. When car goes forward and the steering wheel rotates clockwise viewed from above, steering_angle is positive
If we read the code about assignment to wheel_dist_FL and wheel_dist_BL, in both forward-steering and rear-steering case, we can see that the left side wheels have shorter dist to the circumcenter, therefore in this case the circumcenter is located at left side of car, so car is turning left when going forward. So assumption (a1) is correct from this part of code.
If we read the code about assignment to left_wheel_angle and right_wheel_angle, in forward-steering case, R is positive if steering_angle>0, and left_wheel_angle has a smaller corresponding radius, therefore abs(left_wheel_angle) is larger, left wheel steers more, so car is turning left when going forward. But if it is in rear-steering case, R is negative is steering_angle>0, so abs(left_wheel_angle) is smaller, car is turning right when going forward. So finally the assumption (b1) is correct.
So nothing is correct. The code contradicts to itself. Any idea to fix it ?