Fix OpEn for large vehicle

smarts/core/models/controller_parameters.yaml

@@ -26,15 +26,15 @@ sedan:
 
 bus:
     control:
-            final_heading_gain: 0.1
-            final_lateral_gain: 4.1
+            final_heading_gain: 0.15
+            final_lateral_gain: 5.3
             final_steering_filter_constant: 27.5
             throttle_filter_constant: 2
             velocity_gain: 6.1
             velocity_integral_gain: 0
             traction_gain: 8
-            final_lateral_error_derivative_gain: 0.35
-            final_heading_error_derivative_gain: 2.7
+            final_lateral_error_derivative_gain: 0.45
+            final_heading_error_derivative_gain: 2.5
             initial_look_ahead_distant: 3
             derivative_activation: 1
             speed_reduction_activation: 0
@@ -45,7 +45,7 @@ bus:
             max_brake_gain : 10000
             max_turn_radius : 2.2
             wheel_radius : 0.31265
-            max_torque : 2000
+            max_torque : 2100
             max_btorque : 1200
             max_steering : 12.56
             steering_gear_ratio : 17.4

zoo/policies/open-agent/open_agent/agent.py

@@ -13,14 +13,18 @@
 
 from smarts.core.agent import Agent
 from smarts.core.coordinates import Heading
-
 from .version import VERSION, SOLVER_VERSION
 
 CONFIG_PATH = Path(__file__).parent / "config.json"
 
 
 def angle_error(a, b):
-    return cs.fmin((a - b) ** 2.0, (a - (b + math.pi * 2.0)) ** 2.0)
+
+    return cs.if_else(
+        a - b >= 0,
+        cs.fmin((a - b) ** 2.0, (a - (b + math.pi * 2.0)) ** 2.0),
+        cs.fmin((a - b) ** 2.0, (a - (b - math.pi * 2.0)) ** 2.0),
+    )
 
 
 @dataclass
@@ -202,7 +206,7 @@ def min_cost_by_distance(xrefs: Sequence[XRef], point: XRef, gain: Gain):
     # This calculates the weighted combination of lateral error and
     # heading error, TODO: Define new variable or integrates the coefficents
     # into the default values.
-    weighted_cost = 10 * distant_to_first[3] + 5.1 * cs.fabs(distant_to_first[2])
+    weighted_cost = 10 * distant_to_first[3] + 6.2 * cs.fabs(distant_to_first[2])
     for xref_t in x_ref_iter:
 
         distant_to_point = sum(xref_t.weighted_distance_to(point, gain)[:2])
@@ -215,7 +219,7 @@ def min_cost_by_distance(xrefs: Sequence[XRef], point: XRef, gain: Gain):
         weighted_cost = cs.if_else(
             distant_to_point <= min_xref_t_cost,
             10 * xref_t.weighted_distance_to(point, gain)[3]
-            + 5.1 * cs.fabs(xref_t.weighted_distance_to(point, gain)[2]),
+            + 6.2 * cs.fabs(xref_t.weighted_distance_to(point, gain)[2]),
             weighted_cost,
         )
 

zoo/policies/open-agent/open_agent/config.json

@@ -1 +1 @@
-{"version": "0.1.191", "N": 6, "SV_N": 4, "WP_N": 15, "ts": 0.1}
+{"version": "0.1.192", "N": 6, "SV_N": 4, "WP_N": 15, "ts": 0.1}

zoo/policies/open-agent/open_agent/version.py

@@ -1,3 +1,3 @@
-VERSION = "0.1.191"
+VERSION = "0.1.192"
 
-SOLVER_VERSION = "0.0.5"
+SOLVER_VERSION = "0.0.6"