Fix order of second half-clothoid segment

Author: Gerd Gruenert

traject/plot_smalldelta.py

@@ -24,7 +24,8 @@
 
 
 tparam = TurnParams(1.0, 1.0)
-DELTA = math.pi/8
+# DELTA = math.pi/8
+DELTA = tparam.delta_min*0.9
 
 turn = Turn(tparam, DELTA)
 
@@ -37,18 +38,18 @@
 
 # plot whole line
 XT = np.linspace(0, turn.len, 128, endpoint=True)
-tra = turn.state(XT)
-ax0.plot(tra.x, tra.y, color="yellow", linewidth=5.0, linestyle="-")
+tra1 = turn.state(XT)
+ax0.plot(tra1.x, tra1.y, color="yellow", linewidth=5.0, linestyle="-")
 
 # plot arc segment 1
 X = np.linspace(0, turn._len_clothoid_part_smalldelta, 128, endpoint=True)
-tra = turn._state_clothoid_smalldelta_first(X)
-ax0.plot(tra.x, tra.y, color="red", linewidth=1.0, linestyle="-")
+tra2 = turn._state_clothoid_smalldelta_first(X)
+ax0.plot(tra2.x, tra2.y, color="red", linewidth=1.0, linestyle="-")
 
 # plot arc segment 2
 X = np.linspace(turn._len_clothoid_part_smalldelta, 2*turn._len_clothoid_part_smalldelta, 128, endpoint=True)
-tra = turn._state_clothoid_smalldelta_second(X)
-ax0.plot(tra.x, tra.y, color="green", linewidth=1.0, linestyle="-")
+tra3 = turn._state_clothoid_smalldelta_second(X)
+ax0.plot(tra3.x, tra3.y, color="green", linewidth=1.0, linestyle="-")
 
 
 # plot qg point:

traject/scc/turn.py

@@ -233,8 +233,8 @@ def _state_clothoid_smalldelta_second(self, s):
         assert((s <= self._len_clothoid_part_smalldelta * 2).all())
 
         # original segments: s \in lp .. 2*lp
-        # transform s: for the second clotho part go from 0 to lp
-        my_s = s - self._len_clothoid_part_smalldelta
+        # transform s: for the second clotho part go from lp to 0
+        my_s = 2*self._len_clothoid_part_smalldelta - s
 
         scale = math.sqrt(math.pi / self._sigma_smalldelta)