Problem with my tracked vehicle

[msimanic4-cmd]

I am an amateur in Webots and i need help from professionals. I have a problem with my tracked vehicle. I modified SurveyorSrv1 to my needs, and now when I start my controller it doesn't work correctly. The problem is that when it is moving forward it doesn't rotate the track, it just slides and it doesn't want to turn of course and it is essential for me. Can someone please help me, I'm in a hurry because it is for my paper and without this I have nothing. I will provide first PROTO for my model and then python code i use for my controller which i did with help from Cursor and Chat GPT... Some of the words are in Serbian if you want, just ignore it. It would be a big help for me if you could run it in your Webots and tell me where is my mistake.

Thank all of you in advance.

EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/appearances/protos/Rubber.proto"
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/appearances/protos/BrushedAluminium.proto"

PROTO Jaguar [
field SFVec3f translation 0 0 0 # Is Pose.translation.
field SFRotation rotation 0 0 1 0 # Is Pose.rotation.
field SFString name "Jaguar" # Is Solid.name.
field SFString controller "" # Is Robot.controller.
field MFString controllerArgs [] # Is Robot.controllerArgs.
field SFString window "" # Is Robot.window.
field SFString customData "" # Is Robot.customData.
field SFBool supervisor FALSE # Is Robot.supervisor.
field SFBool synchronization TRUE # Is Robot.synchronization.
field MFNode extensionSlot [] # Extends the robot with new nodes in the extension slot.
]
{
Robot {
translation IS translation
rotation IS rotation
controller IS controller
controllerArgs IS controllerArgs
window IS window
customData IS customData
supervisor IS supervisor
synchronization IS synchronization
name IS name
children [
Group {
children IS extensionSlot
}
DEF BODY Group {
children [
DEF LEFT Track {
translation 0 0.185 0.077
children [
TrackWheel {
position 0.2475 0
radius 0.075
children [
DEF TRACK_WHEEL_SHAPE Shape {
appearance DEF DARK_GREEN PBRAppearance {
baseColor 0.12 0.22 0.12
roughness 0.8
metalness 0.2
}
geometry Cylinder {
height 0.059
radius 0.075
}
}
]
}
TrackWheel {
position -0.2475 0
radius 0.075
children USE TRACK_WHEEL_SHAPE
}
]
name "left track"
contactProperties ContactProperties {
coulombFriction [
2
]
softCFM 0.0001
}
boundingObject DEF TRACK_BO Group {
children [
Pose {
translation 0.2475 0 0
rotation 1 0 0 1.5708
children [
Cylinder {
height 0.059
radius 0.0755
subdivision 20
}
]
}
Pose {
translation -0.2475 0 0
rotation 1 0 0 1.5708
children [
Cylinder {
height 0.059
radius 0.0755
subdivision 20
}
]
}
Box {
size 0.348 0.05 0.148
}
]
}
physics Physics {}
device [
LinearMotor {
name "left motor"
maxForce 300
maxVelocity 2
}
]
animatedGeometry Shape {
appearance Rubber {
}
geometry Box {
size 0.007 0.06 0.002
}
}
geometriesCount 195
}
DEF RIGHT_TRACK Track {
translation 0 -0.185 0.077
children [
TrackWheel {
position 0.2475 0
radius 0.075
children USE TRACK_WHEEL_SHAPE
}
TrackWheel {
position -0.2475 0
radius 0.075
children USE TRACK_WHEEL_SHAPE
}
]
name "right track"
contactProperties ContactProperties {
coulombFriction [
2
]
softCFM 0.0001
}
boundingObject USE TRACK_BO
physics Physics {}
device [
LinearMotor {
name "right motor"
maxForce 300
maxVelocity 2
}
]
animatedGeometry Shape {
appearance Rubber {
}
geometry Box {
size 0.007 0.06 0.002
}
}
geometriesCount 195
}
DEF BASE Pose {
translation 0 0 0.077
children [
Pose {
translation -0.2475 0 0
children [
Shape {
appearance USE DARK_GREEN
geometry Box {
size 0.022 0.3695 0.134
}
}
]
}
Pose {
translation 0.2475 0 0
children [
Shape {
appearance USE DARK_GREEN
geometry Box {
size 0.016 0.3695 0.134
}
}
]
}
Pose {
translation 0 0 0
children [
Shape {
appearance USE DARK_GREEN
geometry Box {
size 0.48 0.29 0.134
}
}
]
}
Pose {
translation -0.2475 0 0
rotation 1 0 0 1.57
children [
DEF WHEEL_AXE Shape {
appearance USE DARK_GREEN
geometry Cylinder {
height 0.37
radius 0.00635
}
}
]
}
Pose {
translation 0.2475 0 0
rotation 1 0 0 1.57
children [
USE WHEEL_AXE
]
}
Pose {
translation -0.25 0 0.517
children [
Shape {
appearance PBRAppearance {
baseColor 0.12 0.22 0.12
roughness 0.8
metalness 0.2
}
geometry Box {
size 0.08 0.1 0.9
}
}
Pose {
translation 0 0 0.4
children [
Camera {
name "camera"
translation 0.025 0 0.01
rotation 1 0 0 -0.05
width 1280
height 720
fieldOfView 1.0
near 0.02
far 500
motionBlur 0.15
noise 0.01
antiAliasing TRUE
}
]
}
]
}
]
}
]
}
]
model "Jaguar"
boundingObject Group {
children [
Pose {
translation 0 0 0.09
children [
Box {
size 0.48 0.28 0.08
}
]
}
]
}
physics Physics {
density -1
mass 3
centerOfMass [
-0.005 0 0.055
]
inertiaMatrix [
0.096 0.171 0.151
0 -5.4e-05 0
]
}
}
}

from controller import Supervisor, Keyboard
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import pandas as pd
import math
import os
import json
import time

DEBUG_LOG = r"C:\Users\Matija.cursor\projects\empty-window\debug-6c2fb2.log"
DEBUG_RUN_ID = "post-fix-2"

def _debug_log(hypothesis_id, location, message, data):
# #region agent log
try:
payload = {
"sessionId": "6c2fb2",
"runId": DEBUG_RUN_ID,
"hypothesisId": hypothesis_id,
"location": location,
"message": message,
"data": data,
"timestamp": int(time.time() * 1000),
}
with open(DEBUG_LOG, "a", encoding="utf-8") as log_file:
log_file.write(json.dumps(payload) + "\n")
except OSError:
pass
# #endregion

=====================================================

PODESAVANJA

=====================================================

TIME_STEP = 64

BASE_SPEED = 0.3
TURN_SPEED = 0.12
BOOST_SPEED = 1.0
TURN_FACTOR = 0.35
TRACK_HALF_WIDTH = 0.185

def _world_yaw(orientation):
return math.atan2(orientation[3], orientation[0])

=====================================================

ROBOT

=====================================================

robot = Supervisor()

keyboard = Keyboard()
keyboard.enable(TIME_STEP)

=====================================================

MOTORI

=====================================================

left_motor = robot.getDevice('left motor')
right_motor = robot.getDevice('right motor')

left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))

left_motor.setAvailableForce(300)
right_motor.setAvailableForce(300)

left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

_debug_log(
"H1",
"zaokret.py:init",
"motor_limits",
{
"leftMaxForce": left_motor.getMaxForce(),
"rightMaxForce": right_motor.getMaxForce(),
"leftMaxVelocity": left_motor.getMaxVelocity(),
"rightMaxVelocity": right_motor.getMaxVelocity(),
},
)

=====================================================

ROBOT NODE

=====================================================

robot_node = robot.getSelf()

=====================================================

OUTPUT

=====================================================

output_dir = "rezultati"

if not os.path.exists(output_dir):
os.makedirs(output_dir)

=====================================================

LISTE

=====================================================

times = []

xs = []
ys = []
zs = []

velocities = []
accelerations = []

yaw_angles = []
angular_velocities = []

distances = []

motion_types = []

=====================================================

POČETNE VREDNOSTI

=====================================================

initial_position = robot_node.getPosition()

x0 = initial_position[0]
y0 = initial_position[1]
z0 = initial_position[2]

previous_position = initial_position

previous_velocity = 0.0
previous_yaw = 0.0

total_distance = 0.0

t = 0.0

=====================================================

ISPIS KOMANDI

=====================================================

print("====================================")
print("KOMANDE")
print("W -> napred")
print("S -> nazad")
print("A -> levo")
print("D -> desno")
print("SHIFT -> ubrzanje")
print("SPACE -> stop")
print("ESC -> kraj simulacije")
print("====================================")

=====================================================

GLAVNA PETLJA

=====================================================

while robot.step(TIME_STEP) != -1:

dt = TIME_STEP / 1000.0
t += dt

# =====================================================
# STANJE TASTERA
# =====================================================

forward = False
backward = False
left = False
right = False
boost = False
stop = False

# =====================================================
# ČITANJE TASTATURE
# =====================================================

key = keyboard.getKey()

while key != -1:

    if key == ord('W'):
        forward = True

    elif key == ord('S'):
        backward = True

    elif key == ord('A'):
        left = True

    elif key == ord('D'):
        right = True

    elif key == Keyboard.SHIFT:
        boost = True

    elif key == ord(' '):
        stop = True

    elif key == Keyboard.END:
        break

    key = keyboard.getKey()

# =====================================================
# BRZINA
# =====================================================

speed = BASE_SPEED

left_speed = 0.0
right_speed = 0.0

if boost:
    speed = BOOST_SPEED

left_speed = 0.0
right_speed = 0.0

# =====================================================
# KRETANJE NAPRED
# =====================================================

if forward:

    left_speed = speed
    right_speed = speed

    if left:
        left_speed = TURN_FACTOR * speed
        right_speed = speed

    elif right:
        left_speed = speed
        right_speed = TURN_FACTOR * speed

# =====================================================
# KRETANJE NAZAD
# =====================================================

elif backward:

    left_speed = -speed
    right_speed = -speed

    if left:
        left_speed = -TURN_FACTOR * speed
        right_speed = -speed

    elif right:
        left_speed = -speed
        right_speed = -TURN_FACTOR * speed

# =====================================================
# OKRETANJE U MESTU
# =====================================================

elif left:

    turn_speed = TURN_SPEED if not boost else min(BOOST_SPEED, TURN_SPEED * 2.0)
    left_speed = -turn_speed
    right_speed = turn_speed

elif right:

    turn_speed = TURN_SPEED if not boost else min(BOOST_SPEED, TURN_SPEED * 2.0)
    left_speed = turn_speed
    right_speed = -turn_speed

# =====================================================
# STOP
# =====================================================

if stop:

    left_speed = 0.0
    right_speed = 0.0

# =====================================================
# POSTAVLJANJE BRZINA
# =====================================================

left_motor.setVelocity(left_speed)
right_motor.setVelocity(right_speed)

# =====================================================
# POZICIJA
# =====================================================

position = robot_node.getPosition()

x = position[0] - x0
y = position[1] - y0
z = position[2] - z0

xs.append(x)
ys.append(y)
zs.append(z)

times.append(t)

# =====================================================
# BRZINA
# =====================================================

dx = position[0] - previous_position[0]
dy = position[1] - previous_position[1]
dz = position[2] - previous_position[2]

distance_step = math.sqrt(dx**2 + dy**2 + dz**2)

velocity = distance_step / dt

velocities.append(velocity)

# =====================================================
# UBRZANJE
# =====================================================

acceleration = (velocity - previous_velocity) / dt

accelerations.append(acceleration)

# =====================================================
# PREĐENI PUT
# =====================================================

total_distance += distance_step

distances.append(total_distance)

# =====================================================
# ORIJENTACIJA
# =====================================================

orientation = robot_node.getOrientation()
robot_velocity = robot_node.getVelocity()

yaw = _world_yaw(orientation)

yaw_angles.append(yaw)

angular_velocity = (yaw - previous_yaw) / dt

angular_velocities.append(angular_velocity)

world_angular_z = robot_velocity[5]
world_speed = math.sqrt(
    robot_velocity[0] ** 2
    + robot_velocity[1] ** 2
    + robot_velocity[2] ** 2
)
left_track_speed = left_motor.getVelocity()
right_track_speed = right_motor.getVelocity()
forward_axis = (
    orientation[0],
    orientation[1],
    orientation[2],
)
body_forward_speed = (
    robot_velocity[0] * forward_axis[0]
    + robot_velocity[1] * forward_axis[1]
    + robot_velocity[2] * forward_axis[2]
)

cmd_linear = 0.5 * (left_speed + right_speed)
cmd_angular = (right_speed - left_speed) / (2.0 * TRACK_HALF_WIDTH)
is_turn_only = abs(cmd_linear) < 0.02 and abs(cmd_angular) > 0.05

slip_linear = 0.0
if abs(cmd_linear) > 0.02:
    slip_linear = abs(world_speed - abs(cmd_linear)) / abs(cmd_linear)

slip_angular = 0.0
if abs(cmd_angular) > 0.05:
    slip_angular = abs(world_angular_z - cmd_angular) / abs(cmd_angular)

if (
    abs(left_speed) > 0.02
    or abs(right_speed) > 0.02
    or abs(cmd_angular) > 0.05
):
    _debug_log(
        "H6",
        "zaokret.py:loop",
        "traction_sample",
        {
            "t": round(t, 3),
            "leftCmd": left_speed,
            "rightCmd": right_speed,
            "leftTrackVel": round(left_track_speed, 4),
            "rightTrackVel": round(right_track_speed, 4),
            "cmdLinear": round(cmd_linear, 4),
            "cmdAngular": round(cmd_angular, 4),
            "actualLinear": round(world_speed, 4),
            "bodyForwardSpeed": round(body_forward_speed, 4),
            "actualAngularZ": round(world_angular_z, 4),
            "yawRate": round(angular_velocity, 4),
            "slipLinear": round(slip_linear, 4),
            "slipAngular": round(slip_angular, 4),
            "isTurnOnly": is_turn_only,
            "boost": boost,
        },
    )

# =====================================================
# TIP KRETANJA
# =====================================================

movement = "Miruje"

if acceleration > 0.8:
    movement = "Ubrzanje"

elif acceleration < -0.8:
    movement = "Kocenje"

elif abs(angular_velocity) > 0.5:
    movement = "Zaokret"

elif dy > 0.002:
    movement = "Uzbrdo"

elif dy < -0.002:
    movement = "Nizbrdo"

elif velocity > 0.05:
    movement = "Pravolinijsko"

motion_types.append(movement)

# =====================================================
# PRETHODNE VREDNOSTI
# =====================================================

previous_position = position
previous_velocity = velocity
previous_yaw = yaw

=====================================================

CSV

=====================================================

df = pd.DataFrame({
"Vreme [s]": times,
"X [m]": xs,
"Y [m]": ys,
"Z [m]": zs,
"Brzina [m/s]": velocities,
"Ubrzanje [m/s2]": accelerations,
"Ugao [rad]": yaw_angles,
"Ugaona brzina [rad/s]": angular_velocities,
"Predjeni put [m]": distances,
"Tip kretanja": motion_types
})

df.to_csv(f"{output_dir}/telemetrija.csv", index=False)

=====================================================

OBOJENA PUTANJA

=====================================================

colors = []

for m in motion_types:

if m == "Pravolinijsko":
    colors.append("green")

elif m == "Uzbrdo":
    colors.append("blue")

elif m == "Nizbrdo":
    colors.append("purple")

elif m == "Zaokret":
    colors.append("orange")

elif m == "Ubrzanje":
    colors.append("red")

elif m == "Kocenje":
    colors.append("black")

else:
    colors.append("gray")

plt.figure(figsize=(12, 8))

for i in range(len(xs)-1):

plt.plot(
    [xs[i], xs[i+1]],
    [zs[i], zs[i+1]],
    color=colors[i],
    linewidth=3
)

plt.xlabel("X koordinata [m]")
plt.ylabel("Z koordinata [m]")

plt.title("Mapa kretanja vozila")

plt.grid()

plt.savefig(f"{output_dir}/mapa_kretanja.png")

=====================================================

VISINA

=====================================================

plt.figure(figsize=(12, 6))

plt.plot(times, ys, linewidth=2)

plt.xlim(2, max(times))

plt.xlabel("Vreme [s]")
plt.ylabel("Visina [m]")

plt.title("Promena visine vozila")

plt.grid()

plt.savefig(f"{output_dir}/visina.png")

=====================================================

BRZINA

=====================================================

plt.figure(figsize=(12, 6))

plt.plot(times, velocities, linewidth=2)

plt.xlim(2, max(times))

plt.xlabel("Vreme [s]")
plt.ylabel("Brzina [m/s]")

plt.title("Brzina vozila")

plt.grid()

plt.savefig(f"{output_dir}/brzina.png")

=====================================================

UBRZANJE

=====================================================

plt.figure(figsize=(12, 6))

plt.plot(times, accelerations, linewidth=2)

plt.xlim(2, max(times))

plt.xlabel("Vreme [s]")
plt.ylabel("Ubrzanje [m/s2]")

plt.title("Ubrzanje vozila")

plt.grid()

plt.savefig(f"{output_dir}/ubrzanje.png")

=====================================================

UGAONA BRZINA

=====================================================

plt.figure(figsize=(12, 6))

plt.plot(times, angular_velocities, linewidth=2)

plt.xlim(2, max(times))

plt.xlabel("Vreme [s]")
plt.ylabel("Ugaona brzina [rad/s]")

plt.title("Zaokret vozila")

plt.grid()

plt.savefig(f"{output_dir}/zaokret.png")

=====================================================

3D PUTANJA

=====================================================

fig = plt.figure(figsize=(12, 10))

ax = fig.add_subplot(111, projection='3d')

ax.plot(xs, zs, ys, linewidth=3)

ax.set_xlabel("X [m]")
ax.set_ylabel("Z [m]")
ax.set_zlabel("Visina [m]")

ax.set_title("3D trajektorija vozila")

plt.savefig(f"{output_dir}/3d_trajektorija.png")

print("====================================")
print("SIMULACIJA ZAVRSENA")
print("Rezultati sacuvani u folder:")
print(output_dir)
print("====================================")