From 0348928e323cf616fffd200f703f08ba8fd7fe7a Mon Sep 17 00:00:00 2001 From: Ahmad Amine Date: Tue, 24 Sep 2024 17:34:33 -0400 Subject: [PATCH] Fix rendering of tires to be correct --- f1tenth_gym/envs/rendering/pyqt_objects.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/f1tenth_gym/envs/rendering/pyqt_objects.py b/f1tenth_gym/envs/rendering/pyqt_objects.py index 046064d2..7f437252 100644 --- a/f1tenth_gym/envs/rendering/pyqt_objects.py +++ b/f1tenth_gym/envs/rendering/pyqt_objects.py @@ -172,9 +172,13 @@ def _get_tire_vertices(pose, length, width, tire_width, tire_length, index, stee vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body """ pose_arr = np.array(pose) - pose_arr[2] = pose_arr[2] + steering - H = get_trmtx(pose_arr) if index == 'fl': + # Shift back, rotate + H_shift = get_trmtx(np.array([-(length/2 - tire_length/2), -(width/2 - tire_width/2), 0])) + H_steer = get_trmtx(np.array([0, 0, steering])) + H_back = get_trmtx(np.array([length/2 - tire_length/2, width/2 - tire_width/2, 0])) + H = get_trmtx(pose_arr) + H = H.dot(H_back).dot(H_steer).dot(H_shift) fl = H.dot(np.asarray([[length / 2], [width / 2], [0.0], [1.0]])).flatten() fr = H.dot(np.asarray([[length / 2], [width / 2 - tire_width], [0.0], [1.0]])).flatten() rr = H.dot(np.asarray([[length / 2 - tire_length], [width / 2 - tire_width], [0.0], [1.0]])).flatten() @@ -187,6 +191,13 @@ def _get_tire_vertices(pose, length, width, tire_width, tire_length, index, stee [[rl[0], rl[1]], [fl[0], fl[1]], [fr[0], fr[1]], [rr[0], rr[1]], [rl[0], rl[1]]] ) elif index == 'fr': + # Shift back, rotate + H_shift = get_trmtx(np.array([-(length/2 - tire_length/2), -(-width/2 + tire_width/2), 0])) + H_steer = get_trmtx(np.array([0, 0, steering])) + H_back = get_trmtx(np.array([length/2 - tire_length/2, -width/2 + tire_width/2, 0])) + H = get_trmtx(pose_arr) + H = H.dot(H_back).dot(H_steer).dot(H_shift) + fl = H.dot(np.asarray([[length / 2], [-width / 2 + tire_width], [0.0], [1.0]])).flatten() fr = H.dot(np.asarray([[length / 2], [-width / 2], [0.0], [1.0]])).flatten() rr = H.dot(np.asarray([[length / 2 - tire_length], [-width / 2], [0.0], [1.0]])).flatten()