Allow right turns

Author: Gerd Gruenert

traject/plot.py

@@ -26,7 +26,7 @@
 
 
 tparam = TurnParams(1.0, 1.0)
-turn = Turn(tparam, math.pi*1.0, 1)
+turn = Turn(tparam, math.pi*1.0)
 
 # plot outer circle:
 omega = tparam.omega
@@ -43,16 +43,16 @@
 
 # plot arc segment
 X = np.linspace(0, tparam.len_clothoid_part, 128, endpoint=True)
-tra = turn.state_clothoid_first(X)
+tra = turn._state_clothoid_first(X)
 ax0.plot(tra.x, tra.y, color="red", linewidth=1.0, linestyle="-")
 
 # plot circle arc segment:
 X2 = np.linspace(tparam.len_clothoid_part, tparam.len_clothoid_part+turn.len_of_circular_part, 128, endpoint=True)
-tra = turn.state_circular(X2)
+tra = turn._state_circular(X2)
 ax0.plot(tra.x, tra.y, color="cyan", linewidth=2.0, linestyle="-")
 
 # plot qi point:
-ax0.plot(tparam.state_qi.x, tparam.state_qi.y, "go")
+ax0.plot(turn.state_qi.x, turn.state_qi.y, "go")
 
 # plot qj point:
 ax0.plot(turn.state_qj.x, turn.state_qj.y, "ro")
@@ -62,7 +62,7 @@
 
 # plot second clothoid:
 X3 = np.linspace(tparam.len_clothoid_part+turn.len_of_circular_part, 2*tparam.len_clothoid_part+turn.len_of_circular_part, 128, endpoint=True)
-tra = turn.state_clothoid_second(X3)
+tra = turn._state_clothoid_second(X3)
 ax0.plot(tra.x, tra.y, color="red", linewidth=1.0, linestyle="-")
 
 # Set x limits, ticks, etc.

traject/scc/turn.py

@@ -16,23 +16,25 @@
 class Turn(object):
     """This generates a turn that is always starting from angle 0, turning to an angle _delta"""
 
-    def __init__(self, _params, _delta, _dir):
+    def __init__(self, _params, _delta):
         """
         Parameters
         ----------
             _params : TurnParams
             _delta : float
-                Deflection of th Turn
+                Deflection of th Turn; positive = left turn, negative = right turn
             _dir : int
                 1 for left turn, -1 for right turn
         """
 
         self.params = _params
-        self.delta = _delta
-        self.dir = _dir
+        self.delta = math.fabs(_delta)
+        self.dir = 1
+        if _delta < 0:
+            self.dir = -1
 
-        assert(_delta > _params.delta_min)
-        assert (_delta < 2 * math.pi)
+        assert(self.delta > _params.delta_min)
+        assert (self.delta < 2 * math.pi)
 
     def state(self, s):
         """
@@ -62,27 +64,27 @@ def state(self, s):
         theta = np.empty(len(s))
         kappa = np.empty(len(s))
 
-        first_clotho = self.state_clothoid_first(s[first_clotho_cond])
+        first_clotho = self._state_clothoid_first(s[first_clotho_cond])
         x[first_clotho_cond] = first_clotho.x
         y[first_clotho_cond] = first_clotho.y
         theta[first_clotho_cond] = first_clotho.theta
         kappa[first_clotho_cond] = first_clotho.kappa
 
-        circ_seg = self.state_circular(s[circ_seg_cond])
+        circ_seg = self._state_circular(s[circ_seg_cond])
         x[circ_seg_cond] = circ_seg.x
         y[circ_seg_cond] = circ_seg.y
         theta[circ_seg_cond] = circ_seg.theta
         kappa[circ_seg_cond] = circ_seg.kappa
 
-        second_clotho = self.state_clothoid_second(s[second_clotho_cond])
+        second_clotho = self._state_clothoid_second(s[second_clotho_cond])
         x[second_clotho_cond] = second_clotho.x
         y[second_clotho_cond] = second_clotho.y
         theta[second_clotho_cond] = second_clotho.theta
         kappa[second_clotho_cond] = second_clotho.kappa
 
-        return State(x, y, theta, kappa)
+        return State(x, self.dir * y, self.dir * theta, self.dir * kappa)
 
-    def state_circular(self, s):
+    def _state_circular(self, s):
         # TODO: Fix the two-clothoids-only case:
         assert (self.delta > self.params.delta_min)
 
@@ -98,8 +100,9 @@ def state_circular(self, s):
 
         return State(x, y, angles, kappa)
 
-    def state_clothoid_first(self, s):
-        """
+    def _state_clothoid_first(self, s):
+        """ left turn, first clothoid segment
+
         Parameters
         ----------
         s : np.array
@@ -121,7 +124,7 @@ def state_clothoid_first(self, s):
 
         return State(scale*ssa_csa[1], scale*ssa_csa[0], theta, kappa)
 
-    def state_clothoid_second(self, s):
+    def _state_clothoid_second(self, s):
         """
         Parameters
         ----------
@@ -145,10 +148,9 @@ def state_clothoid_second(self, s):
         kappa = inv_clothoid_s / self.params.len_clothoid_part * self.params.kappa_max
 
         state = State(-scale * ssa_csa[1], scale * ssa_csa[0], theta, kappa)
-        state = state.rotate_then_translate(self.delta, self.state_qg.x,  self.state_qg.y)
+        state = state.rotate_then_translate(self.delta, self._state_qg.x,  self._state_qg.y)
         return state
 
-
     @cached_property
     def len(self):
         return self.len_of_circular_part + 2*self.params.len_clothoid_part
@@ -165,8 +167,17 @@ def len_of_circular_part(self):
         return 2 * math.pi * self.params.inner_rad * angular_fraction
 
     @cached_property
-    def state_qj(self):
-        """Where the inner circle segment intersects the second clothoid"""
+    def state_qi(self):
+        """Where the first clothoid intersects the inner circle (l/r-turn)"""
+        st = self.params.state_qi
+        st.y *= self.dir
+        st.theta *= self.dir
+        st.kappa *= self.dir
+        return st
+
+    @cached_property
+    def _state_qj(self):
+        """Where the inner circle segment intersects the second clothoid (left-turn)"""
 
         ang = self.delta - self.params.delta_min/2
         x = self.params.omega[0] + self.params.inner_rad * np.sin(ang)
@@ -175,11 +186,30 @@ def state_qj(self):
 
         return State(x, y, ang, kappa)
 
+    @cached_property
+    def state_qj(self):
+        """Where the inner circle segment intersects the second clothoid (l/r-turn)"""
+        st = self._state_qj
+        st.y *= self.dir
+        st.theta *= self.dir
+        st.kappa *= self.dir
+        return st
+
     @cached_property
     def state_qg(self):
-        """The end of the second clothoid"""
+        """The end of the second clothoid (turn left | right for dir=1 | -1)"""
+        st = self._state_qg
+        st.y *= self.dir
+        st.theta *= self.dir
+        st.kappa *= self.dir
+        return st
+
+    @cached_property
+    def _state_qg(self):
+        """The end of the second clothoid (left-turn)"""
 
         st = State(-self.params.omega[0], -self.params.omega[1], self.delta, 0)
-        st = st.rotate_then_translate(self.delta+2*self.params.gamma, self.params.omega[0], self.params.omega[1])
+        st = st.rotate_then_translate(self.delta + 2 * self.params.gamma, self.params.omega[0], self.params.omega[1])
 
         return st
+

traject/scc/turnparams.py

@@ -33,20 +33,26 @@ def inner_rad(self):
 
     @cached_property
     def outer_rad(self):
-        return math.sqrt(self.omega[0]*self.omega[0] + self.omega[1]*self.omega[1])
+        return math.sqrt(self.omega[0] * self.omega[0] + self.omega[1] * self.omega[1])
 
     @cached_property
     def omega(self):
-        """The position of the center of the outer/inner circle."""
+        """The position of the center of the outer/inner circle.  (left-turn)"""
         x_qi = self.state_qi.x
         y_qi = self.state_qi.y
         xo = x_qi - math.sin(self.state_qi.theta) / self.kappa_max
         yo = y_qi + math.cos(self.state_qi.theta) / self.kappa_max
         return xo, yo
 
+    @cached_property
+    def omega_r(self):
+        """The position of the center of the outer/inner circle of a right-turn."""
+        xo, yo = self.omega
+        return xo, -yo
+
     @cached_property
     def state_qi(self):
-        """Where the first clothoid intersects the inner circle"""
+        """Where the first clothoid intersects the inner circle (left-turn)"""
         scale = math.sqrt(math.pi / self.sigma_max)
 
         ssa_csa = scipy.special.fresnel(math.sqrt(self.delta_min/math.pi))
@@ -61,8 +67,15 @@ def state_qi(self):
         )
         return st
 
+    @cached_property
+    def state_qi_r(self):
+        """Where the first clothoid intersects the inner circle in a right-turn"""
+        s = self.state_qi
+        s.y *= -1
+        return s
+
     @cached_property
     def gamma(self):
         """The angle between the outer circle tangent and the start/end vector."""
-        gamma = math.atan(self.omega[0]/self.omega[1])
+        gamma = math.atan(self.omega[0] / self.omega[1])
         return gamma