Moving an external CAD model according to defined waypoints

Hello All,

Here I have imported an external CAD model of a car and have assigned it as a Rigid body.
All I want to do is move this rigid body using a python script so that I can define specific XY coordinates where the car should move and it turns and stops accordingly so that it looks like some kind of simulation to the specific waypoints where a car accelerates, reach a certain velocity and stops once it has reached a given waypoint.

What I could manage was that I could move the car at a constant velocity using a python script so that the car could move at a constant velocity.

Here is the script:

from pxr import Gf, UsdPhysics, Usd, UsdGeom
import omni.usd

stage = omni.usd.get_context().get_stage()
rigid_body_api = UsdPhysics.RigidBodyAPI.Get(stage, "/World/Camaro_68_010")
if rigid_body_api:
    vel = Gf.Vec3f(0,-20,0)
    ang_vel = Gf.Vec3f(0,0,10)
    rigid_body_api.GetVelocityAttr().Set(vel)
    rigid_body_api.GetAngularVelocityAttr().Set(ang_vel)

How can I include the information about the waypoints in the above script. E.g. I wish to move the car to the following coordinates:
waypoints = [
(0, 0),
(0, 5),
(0, 10),
(5, 10),
(5, 0)
]

I tried something like below but the car didn’t move from its position. Here’s what I tried:

from pxr import Gf, UsdPhysics, Usd, UsdGeom
import omni.usd
import time

stage = omni.usd.get_context().get_stage()

rigid_body_path = "/World/Camaro_68_010"
rigid_body_api = UsdPhysics.RigidBodyAPI.Get(stage, rigid_body_path)

waypoints = [
    (0, 0),
    (0, 5),
    (0, 10),
    (5, 10),
    (5, 0)
]

velocity_magnitude = 5.0  # m/s
time_interval = 0.1  # seconds

def move_rigid_body_through_waypoints():

    for waypoint in waypoints:
        current_position = stage.GetPrimAtPath(rigid_body_path).GetAttribute("xformOp:translate").Get()
        current_position = Gf.Vec3f(current_position[0], current_position[1], current_position[2])
        
        target_position = Gf.Vec3f(waypoint[0], waypoint[1], current_position[2])  # Maintain the same Z value
        direction = target_position - current_position
        distance = direction.GetLength()
        direction.Normalize()

        velocity = direction * velocity_magnitude
        rigid_body_api.GetVelocityAttr().Set(velocity)

        time_to_reach = distance / velocity_magnitude
        time.sleep(time_to_reach)

        rigid_body_api.GetVelocityAttr().Set(Gf.Vec3f(0, 0, 0))

        print(f"Reached waypoint {waypoint}: Position - {target_position}")

        angular_velocity = Gf.Vec3f(0, 0, 10)  
        rigid_body_api.GetAngularVelocityAttr().Set(angular_velocity)
        time.sleep(0.1)  
        rigid_body_api.GetAngularVelocityAttr().Set(Gf.Vec3f(0, 0, 0))

move_rigid_body_through_waypoints()

Thank You