changing desired speed for PID back to derivative eval and changing max speed back to 10.0

Author: diegoasfmravpp

GEMstack/knowledge/vehicle/gem_e2_fast_limits.yaml

@@ -1,5 +1,5 @@
 max_steering_rate: 4.0       #radians/s
-max_speed: 6.0   #m/s, approx 20 mph
+max_speed: 10.0   #m/s, approx 20 mph
 max_reverse_speed: 2.5   #m/s, approx 5 mph
 max_acceleration: 3.0   #m/s^2
 max_deceleration: 5.0   #m/s^2

GEMstack/onboard/planning/stanley.py

@@ -220,44 +220,29 @@ def compute(self, state: VehicleState, component: Component = None):
                 desired_steering_angle = 0.0
 
             else:
-                #  Use racing velocity profile as target
-                if  self.desired_speed_source == 'racing':
-                    desired_speed = self.trajectory.velocities[index]
-                    desired_speed = min(desired_speed, self.speed_limit)
-                    
-                    next_desired_speed = self.trajectory.velocities[index + 1]
-                    next_desired_speed = min(next_desired_speed, self.speed_limit)
-                    difference_dt = self.trajectory.times[index + 1] - self.trajectory.times[index]
-                    
-                    feedforward_accel = (next_desired_speed - desired_speed) / difference_dt
-                    feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel)
-                    
-                    if speed < next_desired_speed and feedforward_accel < 0:
-                        feedforward_accel = 0.0
+                if self.desired_speed_source == 'path':
+                    current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter)
                 else:
-                    if self.desired_speed_source == 'path':
-                        current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter)
-                    else:
-                        self.current_traj_parameter += dt
-                        current_trajectory_time = self.current_traj_parameter
-                        print(current_trajectory_time)
-
-                    deriv = self.trajectory.eval_derivative(current_trajectory_time)
-
-                    # deriv 0 at time 0
-                    if np.isnan(deriv[0]):
-                        desired_speed = 0.0
-                    else:
-                        desired_speed = min(np.linalg.norm(deriv), self.speed_limit)
-
-                    difference_dt = 0.1
-                    future_t = current_trajectory_time + difference_dt
-                    if future_t > self.trajectory.domain()[1]:
-                        future_t = self.trajectory.domain()[1]
-                    future_deriv = self.trajectory.eval_derivative(future_t)
-                    next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit)
-                    feedforward_accel = (next_desired_speed - desired_speed) / difference_dt
-                    feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel)
+                    self.current_traj_parameter += dt
+                    current_trajectory_time = self.current_traj_parameter
+                    print(current_trajectory_time)
+
+                deriv = self.trajectory.eval_derivative(current_trajectory_time)
+
+                # deriv 0 at time 0
+                if np.isnan(deriv[0]):
+                    desired_speed = 0.0
+                else:
+                    desired_speed = min(np.linalg.norm(deriv), self.speed_limit)
+
+                difference_dt = 0.1
+                future_t = current_trajectory_time + difference_dt
+                if future_t > self.trajectory.domain()[1]:
+                    future_t = self.trajectory.domain()[1]
+                future_deriv = self.trajectory.eval_derivative(future_t)
+                next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit)
+                feedforward_accel = (next_desired_speed - desired_speed) / difference_dt
+                feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel)
         else:
             if desired_speed is None:
                 desired_speed = 4.0

GEMstack/state/trajectory.py

@@ -88,7 +88,7 @@ def racing_velocity_profile(self) -> Trajectory:
         times = [0.0]
         points = self.points
         times, velocities = compute_velocity_profile(points)
-        return Trajectory(frame=self.frame,points=points,times=times, velocities=velocities)
+        return Trajectory(frame=self.frame,points=points,times=times)
 
     def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]:
         """Returns the closest point on the path to the given point.  If
@@ -273,7 +273,6 @@ def det(matrix):
 class Trajectory(Path):
     """A timed, piecewise linear path."""
     times : List[float]
-    velocities : Optional[List[float]] = None 
 
     def domain(self) -> Tuple[float,float]:
         """Returns the time parameter domain"""