Add vector operators, start fixing scripts for rotated map

Author: shalinmehtalgsvl

lgsvl/geometry.py

@@ -21,13 +21,33 @@ def __repr__(self):
     return "Vector({}, {}, {})".format(self.x, self.y, self.z)
 
   def __add__(self, v):
-    return Vector(self.x + v.x, self.y + v.y, self.z + v.z)
+    if isinstance(v, Vector):
+      return Vector(self.x + v.x, self.y + v.y, self.z + v.z)
+
+    if isinstance(v, (int, float)):
+      return Vector(self.x + v, self.y + v, self.z + v)
 
   def __sub__(self, v):
-    return Vector(self.x - v.x, self.y - v.y, self.z - v.z)
+    if isinstance(v, Vector):
+      return Vector(self.x - v.x, self.y - v.y, self.z - v.z)
+    
+    if isinstance(v, (int, float)):
+      return Vector(self.x - v, self.y - v, self.z - v)
 
   def __mul__(self, v):
-    return Vector(self.x * v.x, self.y * v.y, self.z * v.z)
+    if isinstance(v, Vector):
+      return Vector(self.x * v.x, self.y * v.y, self.z * v.z)
+    
+    if isinstance(v, (int, float)):
+      return Vector(self.x * v, self.y * v, self.z * v)
+
+  def __rmul__(self, v):
+    return self * v
+    # if isinstance(v, Vector):
+    #   return Vector(self.x * v.x, self.y * v.y, self.z * v.z)
+    
+    # if isinstance(v, (int, float)):
+    #   return Vector(self.x * v, self.y * v, self.z * v)
 
 class BoundingBox:
   def __init__(self, min, max):

lgsvl/utils.py

@@ -44,6 +44,38 @@ def transform_to_matrix(tr):
            [ px, py, pz, 1.0 ],
          ]
 
+def transform_to_forward(tr):
+  ax = tr.rotation.x * math.pi / 180.0
+  sx, cx = math.sin(ax), math.cos(ax)
+
+  ay = tr.rotation.y * math.pi / 180.0
+  sy, cy = math.sin(ay), math.cos(ay)
+
+  return Vector(cx * sy, -sx, cx * cy)
+
+def transform_to_up(tr):
+  ax = tr.rotation.x * math.pi / 180.0
+  ay = tr.rotation.y * math.pi / 180.0
+  az = tr.rotation.z * math.pi / 180.0
+
+  sx, cx = math.sin(ax), math.cos(ax)
+  sy, cy = math.sin(ay), math.cos(ay)
+  sz, cz = math.sin(az), math.cos(az)
+
+  return Vector(sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz)
+
+def transform_to_right(tr):
+  ax = tr.rotation.x * math.pi / 180.0
+  ay = tr.rotation.y * math.pi / 180.0
+  az = tr.rotation.z * math.pi / 180.0
+
+  sx, cx = math.sin(ax), math.cos(ax)
+  sy, cy = math.sin(ay), math.cos(ay)
+  sz, cz = math.sin(az), math.cos(az)
+
+  return Vector(sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz)
+
+
 
 def vector_dot(a, b):
   return a.x * b.x + a.y * b.y + a.z * b.z

quickstart/04-ego-drive-straight.py

@@ -7,6 +7,7 @@
 
 import os
 import lgsvl
+import math
 
 sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181)
 if sim.current_scene == "BorregasAve":
@@ -18,8 +19,11 @@
 
 state = lgsvl.AgentState()
 state.transform = spawns[0]
+
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+
 # Agents can be spawned with a velocity. Default is to spawn with 0 velocity
-state.velocity = lgsvl.Vector(0, 0, 20)
+state.velocity = 20 * forward
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
 # The bounding box of an agent are 2 points (min and max) such that the box formed from those 2 points completely encases the agent

quickstart/05-ego-drive-in-circle.py

@@ -18,7 +18,8 @@
 
 state = lgsvl.AgentState()
 state.transform = spawns[0]
-state.transform.position.z += 5 # 5m forwards
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+state.transform.position += 5 * forward# 5m forwards
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
 print("Current time = ", sim.current_time)

quickstart/08-create-npc.py

@@ -21,17 +21,16 @@
 state.transform = spawns[0]
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
-# Spawn NPC vehicles 10 meters ahead of the EGO
-sx = spawns[0].position.x
-sz = spawns[0].position.z + 10.0
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
 
 # Spawns one of each of the listed types of NPCS
 # The first will be created in front of the EGO and then they will be created to the left
 # The available types of NPCs can be found in NPCManager prefab
 for i, name in enumerate(["Sedan", "SUV", "Jeep", "Hatchback"]):
   state = lgsvl.AgentState()
-  state.transform = spawns[0]
 
-  state.transform.position.x = sx - 4.0 * i
-  state.transform.position.z = sz
+  # Spawn NPC vehicles 10 meters ahead of the EGO
+  state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right)
+  state.transform.rotation = spawns[0].rotation
   sim.add_agent(name, lgsvl.AgentType.NPC, state)

quickstart/09-reset-scene.py

@@ -25,16 +25,15 @@
 state.transform = spawns[0]
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
-# 10 meters ahead
-sx = state.transform.position.x
-sz = state.transform.position.z + 10.0
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
+
 
 for i, name in enumerate(["Sedan", "SUV", "Jeep", "Hatchback"]):
   state = lgsvl.AgentState()
-  state.transform = spawns[0]
-
-  state.transform.position.x = sx - 4.0 * i
-  state.transform.position.z = sz
+  # 10 meters ahead
+  state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right)
+  state.transform.rotation = spawns[0].rotation
   sim.add_agent(name, lgsvl.AgentType.NPC, state)
 
 input("Press Enter to reset")

quickstart/10-npc-follow-the-lane.py

@@ -21,22 +21,21 @@
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
 state = lgsvl.AgentState()
-state.transform = spawns[0]
 
-sx = spawns[0].position.x
-sz = spawns[0].position.z
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
 
 # 10 meters ahead, on left lane
-state.transform.position.z = sz + 10.0
+state.transform.position = spawns[0].position + 10.0 * forward
+state.transform.rotation = spawns[0].rotation
 
 npc1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
 
 state = lgsvl.AgentState()
-state.transform = spawns[0]
 
 # 10 meters ahead, on right lane
-state.transform.position.x = sx + 4.0
-state.transform.position.z = sz + 10.0
+state.transform.position = spawns[0].position + 4.0 * right + 10.0 * forward
+state.transform.rotation = spawns[0].rotation
 
 npc2 = sim.add_agent("SUV", lgsvl.AgentType.NPC, state)
 

quickstart/11-collision-callbacks.py

@@ -26,19 +26,20 @@
 state.transform = spawns[0]
 ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
+
 # school bus, 20m ahead, perpendicular to road, stopped
 
 state = lgsvl.AgentState()
-state.transform = spawns[0]
-state.transform.position.z = sz + 20.0
-state.transform.rotation.y = 90.0
+state.transform.position = spawns[0].position + 20.0 * forward
+state.transform.rotation.y = spawns[0].rotation.y + 90.0
 bus = sim.add_agent("SchoolBus", lgsvl.AgentType.NPC, state)
 
 # sedan, 10m ahead, driving forward
 state = lgsvl.AgentState()
-state.transform = spawns[0]
-state.transform.position.z = sz + 10.0
-state.transform.rotation.y = ry
+state.transform.position = spawns[0].position + 10.0 * forward
+state.transform.rotation = spawns[0].rotation
 sedan = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
 # Even though the sedan is commanded to follow the lane, obstacle avoidance takes precedence and it will not drive into the bus
 sedan.follow_closest_lane(True, 11.1) # 11.1 m/s is ~40 km/h

quickstart/13-npc-follow-waypoints.py

@@ -17,13 +17,14 @@
   sim.load("BorregasAve")
 
 spawns = sim.get_spawn()
-
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
 # EGO
 
 state = lgsvl.AgentState()
 state.transform = spawns[0]
 state2 = copy.deepcopy(state)
-state2.transform.position.z += 50
+state2.transform.position += 50 * forward
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state2)
 
 # NPC, 10 meters ahead
@@ -33,9 +34,8 @@
 sz = spawns[0].position.z + 10.0
 
 state = lgsvl.AgentState()
-state.transform = spawns[0]
-state.transform.position.x = sx
-state.transform.position.z = sz
+state.transform.position = spawns[0].position + 10 * forward
+state.transform.rotation = spawns[0].rotation
 npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state)
 
 vehicles = {
@@ -66,9 +66,9 @@ def on_collision(agent1, agent2, contact):
   px = 0
   pz = (i + 1) * z_delta
   # Waypoint angles are input as Euler angles (roll, pitch, yaw)
-  angle = lgsvl.Vector(0, 0, 0)
+  angle = spawns[0].rotation
   # Raycast the points onto the ground because BorregasAve is not flat
-  hit = sim.raycast(lgsvl.Vector(sx + px, sy, sz + pz), lgsvl.Vector(0,-1,0), layer_mask) 
+  hit = sim.raycast(spawns[0].position + pz * forward, lgsvl.Vector(0,-1,0), layer_mask) 
 
   # NPC will wait for 1 second at each waypoint
   wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1, 0)

quickstart/14-create-pedestrians.py

@@ -20,6 +20,8 @@
 
 state = lgsvl.AgentState()
 state.transform = spawns[0]
+forward = lgsvl.utils.transform_to_forward(spawns[0])
+right = lgsvl.utils.transform_to_right(spawns[0])
 a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state)
 
 sx = state.transform.position.x
@@ -33,10 +35,9 @@
 i = 0
 while True:
   state = lgsvl.AgentState()
-  state.transform = spawns[0]
 
-  state.transform.position.x = sx + 5 - (1.0 * (i % 16))
-  state.transform.position.z = sz + 5 + (1.0 * (i//16))
+  state.transform.position = spawns[0].position + (5 + (1.0 * (i//16))) * forward + (5 - (1.0 * (i % 16))) * right
+  state.transform.rotation = spawns[0].rotation
   name = random.choice(names)
   print("({}) adding {}".format(i+1, name))
   p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state)