Skip to content

Commit 02577a5

Browse files
JugthegreatJugthegreat
authored
Updated stanley.py
Basic version of Stanley. Not yet tested on GEM vehicle.
1 parent fb2b17e commit 02577a5

1 file changed

Lines changed: 101 additions & 140 deletions

File tree

‎GEMstack/onboard/planning/stanley.py‎

Lines changed: 101 additions & 140 deletions
Original file line numberDiff line numberDiff line change
@@ -2,242 +2,203 @@
22
from ...utils import settings
33
from ...mathutils import transforms
44
from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions
5-
from ...state.vehicle import VehicleState,ObjectFrameEnum
6-
from ...state.trajectory import Path,Trajectory,compute_headings
5+
from ...state.vehicle import VehicleState, ObjectFrameEnum
6+
from ...state.trajectory import Path, Trajectory, compute_headings
77
from ...knowledge.vehicle.geometry import front2steer
88
from ..interface.gem import GEMVehicleCommand
99
from ..component import Component
1010
import numpy as np
1111

12-
class PurePursuit(object):
12+
class PurePursuit(object):
1313
"""Implements a pure pursuit controller on a second-order Dubins vehicle."""
14-
def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = None, desired_speed = None):
15-
self.look_ahead = lookahead if lookahead is not None else settings.get('control.pure_pursuit.lookahead',4.0)
16-
self.look_ahead_scale = lookahead_scale if lookahead_scale is not None else settings.get('control.pure_pursuit.lookahead_scale',3.0)
17-
self.crosstrack_gain = crosstrack_gain if crosstrack_gain is not None else settings.get('control.pure_pursuit.crosstrack_gain',0.41)
14+
# ... [Keep existing PurePursuit implementation unchanged] ...
15+
16+
class StanleyController(object):
17+
"""Implements a Stanley controller for path tracking."""
18+
def __init__(self, k_gain=None, soft_gain=None, desired_speed=None):
19+
self.k_gain = k_gain if k_gain is not None else settings.get('control.stanley.k_gain', 2.5)
20+
self.soft_gain = soft_gain if soft_gain is not None else settings.get('control.stanley.soft_gain', 0.1)
1821
self.front_wheel_angle_scale = 3.0
19-
self.wheelbase = settings.get('vehicle.geometry.wheelbase')
20-
self.wheel_angle_range = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')]
21-
self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')]
22+
self.wheelbase = settings.get('vehicle.geometry.wheelbase')
23+
self.wheel_angle_range = [settings.get('vehicle.geometry.min_wheel_angle'),
24+
settings.get('vehicle.geometry.max_wheel_angle')]
25+
self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),
26+
settings.get('vehicle.geometry.max_steering_angle')]
2227

2328
if desired_speed is not None:
2429
self.desired_speed_source = desired_speed
2530
else:
26-
self.desired_speed_source = settings.get('control.pure_pursuit.desired_speed',"path")
27-
self.desired_speed = self.desired_speed_source if isinstance(self.desired_speed_source,(int,float)) else None
31+
self.desired_speed_source = settings.get('control.stanley.desired_speed', "path")
32+
self.desired_speed = self.desired_speed_source if isinstance(self.desired_speed_source, (int, float)) else None
2833
self.speed_limit = settings.get('vehicle.limits.max_speed')
29-
self.max_accel = settings.get('vehicle.limits.max_acceleration') # m/s^2
30-
self.max_decel = settings.get('vehicle.limits.max_deceleration') # m/s^2
31-
self.pid_speed = PID(settings.get('control.longitudinal_control.pid_p',0.5), settings.get('control.longitudinal_control.pid_d',0.0), settings.get('control.longitudinal_control.pid_i',0.1), windup_limit=20)
34+
self.max_accel = settings.get('vehicle.limits.max_acceleration') # m/s^2
35+
self.max_decel = settings.get('vehicle.limits.max_deceleration') # m/s^2
36+
self.pid_speed = PID(settings.get('control.longitudinal_control.pid_p', 0.5),
37+
settings.get('control.longitudinal_control.pid_d', 0.0),
38+
settings.get('control.longitudinal_control.pid_i', 0.1),
39+
windup_limit=20)
3240

3341
self.path_arg = None
34-
self.path = None # type: Trajectory
35-
#if trajectory = None, then only an untimed path was provided and we can't use the path velocity as the reference
42+
self.path = None # type: Trajectory
3643
self.trajectory = None # type: Trajectory
37-
3844
self.current_path_parameter = 0.0
3945
self.current_traj_parameter = 0.0
4046
self.t_last = None
4147

42-
def set_path(self, path : Path):
48+
def set_path(self, path: Path):
4349
if path == self.path_arg:
4450
return
4551
self.path_arg = path
4652
if len(path.points[0]) > 2:
47-
path = path.get_dims([0,1])
48-
if not isinstance(path,Trajectory):
53+
path = path.get_dims([0, 1])
54+
if not isinstance(path, Trajectory):
4955
self.path = path.arc_length_parameterize()
5056
self.trajectory = None
5157
self.current_traj_parameter = 0.0
52-
if self.desired_speed_source in ['path','trajectory']:
53-
raise ValueError("Can't provide an untimed path to PurePursuit and expect it to use the path velocity. Set control.pure_pursuit.desired_speed to a constant.")
58+
if self.desired_speed_source in ['path', 'trajectory']:
59+
raise ValueError("Can't provide an untimed path to StanleyController and expect it to use the path velocity.")
5460
else:
5561
self.path = path.arc_length_parameterize()
5662
self.trajectory = path
5763
self.current_traj_parameter = self.trajectory.domain()[0]
5864
self.current_path_parameter = 0.0
5965

60-
def compute(self, state : VehicleState, component : Component = None):
66+
def compute(self, state: VehicleState, component: Component = None):
6167
assert state.pose.frame != ObjectFrameEnum.CURRENT
6268
t = state.pose.t
6369

6470
if self.t_last is None:
6571
self.t_last = t
6672
dt = t - self.t_last
67-
73+
6874
curr_x = state.pose.x
6975
curr_y = state.pose.y
7076
curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0
7177
speed = state.v
7278

7379
if self.path is None:
74-
#just stop
80+
# just stop
7581
accel = self.pid_speed.advance(0.0, t)
76-
# TODO
7782
raise RuntimeError("Behavior without path not implemented")
7883

7984
if self.path.frame != state.pose.frame:
80-
print("Transforming path from",self.path.frame.name,"to",state.pose.frame.name)
85+
print("Transforming path from", self.path.frame.name, "to", state.pose.frame.name)
8186
self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose)
8287
if self.trajectory is not None:
8388
if self.trajectory.frame != state.pose.frame:
84-
print("Transforming trajectory from",self.trajectory.frame.name,"to",state.pose.frame.name)
89+
print("Transforming trajectory from", self.trajectory.frame.name, "to", state.pose.frame.name)
8590
self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose)
8691

87-
closest_dist,closest_parameter = self.path.closest_point_local((curr_x,curr_y),[self.current_path_parameter-5.0,self.current_path_parameter+5.0])
92+
# Find closest point on path
93+
closest_dist, closest_parameter = self.path.closest_point_local(
94+
(curr_x, curr_y), [self.current_path_parameter - 5.0, self.current_path_parameter + 5.0])
8895
self.current_path_parameter = closest_parameter
8996
self.current_traj_parameter += dt
90-
#TODO: calculate parameter that is look_ahead distance away from the closest point?
91-
#(rather than just advancing the parameter)
92-
93-
#Stanley
94-
des_parameter = closest_parameter + self.look_ahead
95-
96-
#Pure pursuit
97-
#des_parameter = closest_parameter + self.look_ahead + self.look_ahead_scale * speed
98-
99-
print("Closest parameter: " + str(closest_parameter),"distance to path",closest_dist)
100-
if closest_dist > 0.1:
101-
print("Closest point",self.path.eval(closest_parameter),"vs",(curr_x,curr_y))
102-
if des_parameter >= self.path.domain()[1]:
103-
#we're at the end of the path, calculate desired point by extrapolating from the end of the path
104-
end_pt = self.path.points[-1]
105-
if len(self.path.points) > 1:
106-
end_dir = self.path.eval_tangent(self.path.domain()[1])
107-
else:
108-
#path is just a single point, just look at current direction
109-
end_dir = (np.cos(curr_yaw),np.sin(curr_yaw))
110-
desired_x,desired_y = transforms.vector_madd(end_pt,end_dir,(des_parameter-self.path.domain()[1]))
111-
else:
112-
desired_x,desired_y = self.path.eval(des_parameter)
113-
desired_yaw = np.arctan2(desired_y-curr_y,desired_x-curr_x)
114-
#print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed)
115-
#print("Current yaw",curr_yaw,"desired yaw",desired_yaw)
116-
11797

118-
# distance between the desired point and the vehicle
119-
L = transforms.vector2_dist((desired_x,desired_y),(curr_x,curr_y))
120-
121-
# find the curvature and the angle
122-
alpha = transforms.normalize_angle(desired_yaw - curr_yaw)
123-
if alpha > np.pi:
124-
alpha -= np.pi*2
125-
126-
## STANLEY
127-
closest_x,closest_y = self.path.eval(closest_parameter)
98+
# Get path point and tangent at closest point
99+
path_point = self.path.eval(closest_parameter)
100+
path_tangent = self.path.eval_tangent(closest_parameter)
101+
path_yaw = np.arctan2(path_tangent[1], path_tangent[0])
128102

103+
# Calculate crosstrack error (e) - signed distance from front axle to path
104+
front_x = curr_x + self.wheelbase * np.cos(curr_yaw)
105+
front_y = curr_y + self.wheelbase * np.sin(curr_yaw)
129106

130-
e_numerator = (desired_x-closest_x) * (closest_y-curr_y) - (closest_x-curr_x) * (desired_y-closest_y)
131-
132-
e_denominator = np.sqrt((desired_x-closest_x)**2 + (desired_y-closest_y)**2)
133-
134-
e = e_numerator / e_denominator
135-
136-
k = self.crosstrack_gain
137-
138-
crosstrack_steering = np.arctan2(k * e, speed)
139-
heading_error = alpha # change to (desired_to_closest_yaw - curr_yaw)
140-
141-
angle_i = alpha + crosstrack_steering
142-
angle = angle_i*self.front_wheel_angle_scale
143-
144-
## Pure pursuit
145-
# ----------------- tuning this part as needed -----------------
146-
147-
# k = self.crosstrack_gain
148-
# angle_i = np.arctan((k * 2 * self.wheelbase * np.sin(alpha)) / L)
149-
# angle = angle_i*self.front_wheel_angle_scale
150-
# ----------------- tuning this part as needed -----------------
151-
152-
f_delta = np.clip(angle, self.wheel_angle_range[0], self.wheel_angle_range[1])
107+
# Vector from path point to front axle
108+
dx = front_x - path_point[0]
109+
dy = front_y - path_point[1]
153110

154-
#print("Closest point distance: " + str(L))
155-
print("Forward velocity: " + str(speed))
156-
ct_error = np.sin(alpha) * L
157-
print("Crosstrack Error: " + str(round(ct_error,3)))
158-
print("Front wheel angle: " + str(round(np.degrees(f_delta),2)) + " degrees")
111+
# Calculate perpendicular distance (crosstrack error)
112+
# Cross product between path tangent and vector to front axle gives signed distance
113+
e = (dx * path_tangent[1] - dy * path_tangent[0]) / np.linalg.norm(path_tangent)
114+
115+
# Calculate heading error (θ)
116+
theta_e = transforms.normalize_angle(path_yaw - curr_yaw)
117+
118+
# Stanley control law
119+
# Avoid division by zero with soft_gain term
120+
crosstrack_term = np.arctan2(self.k_gain * e, speed + self.soft_gain)
121+
f_delta = theta_e + crosstrack_term
122+
f_delta = np.clip(f_delta, self.wheel_angle_range[0], self.wheel_angle_range[1])
123+
124+
# Debug outputs
125+
print("Crosstrack Error: " + str(round(e, 3)))
126+
print("Heading Error: " + str(round(np.degrees(theta_e), 2)) + " degrees")
127+
print("Front wheel angle: " + str(round(np.degrees(f_delta), 2) + " degrees")
159128
steering_angle = np.clip(front2steer(f_delta), self.steering_angle_range[0], self.steering_angle_range[1])
160-
print("Steering wheel angle: " + str(round(np.degrees(steering_angle),2)) + " degrees" )
161-
129+
print("Steering wheel angle: " + str(round(np.degrees(steering_angle), 2)) + " degrees")
130+
131+
# Speed control (same as PurePursuit)
162132
desired_speed = self.desired_speed
163133
feedforward_accel = 0.0
164-
if self.desired_speed_source in ['path','trajectory']:
165-
#determine desired speed from trajectory
134+
if self.desired_speed_source in ['path', 'trajectory']:
166135
if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]:
167136
if component is not None:
168137
component.debug_event('Past the end of trajectory')
169-
#past the end, just stop
170138
desired_speed = 0.0
171139
feedforward_accel = -2.0
172140
else:
173-
if self.desired_speed_source=='path':
141+
if self.desired_speed_source == 'path':
174142
current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter)
175143
else:
176144
current_trajectory_time = self.current_traj_parameter
177145
deriv = self.trajectory.eval_derivative(current_trajectory_time)
178-
desired_speed = min(np.linalg.norm(deriv),self.speed_limit)
146+
desired_speed = min(np.linalg.norm(deriv), self.speed_limit)
179147
difference_dt = 0.1
180148
if current_trajectory_time >= self.trajectory.domain()[1]:
181149
prev_deriv = self.trajectory.eval_derivative(current_trajectory_time - difference_dt)
182-
prev_desired_speed = min(np.linalg.norm(prev_deriv),self.speed_limit)
150+
prev_desired_speed = min(np.linalg.norm(prev_deriv), self.speed_limit)
183151
feedforward_accel = (desired_speed - prev_desired_speed)/difference_dt
184-
print("Desired speed",desired_speed,"m/s",", from prior",prev_desired_speed,"m/s")
185152
else:
186153
next_deriv = self.trajectory.eval_derivative(current_trajectory_time + difference_dt)
187-
next_desired_speed = min(np.linalg.norm(next_deriv),self.speed_limit)
154+
next_desired_speed = min(np.linalg.norm(next_deriv), self.speed_limit)
188155
feedforward_accel = (next_desired_speed - desired_speed)/difference_dt
189-
print("Desired speed",desired_speed,"m/s",", trying to reach desired",next_desired_speed,"m/s")
190-
feedforward_accel= np.clip(feedforward_accel, -self.max_decel, self.max_accel)
191-
print("Feedforward accel: " + str(feedforward_accel) + " m/s^2")
156+
feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel)
192157
else:
193-
#decay speed when crosstrack error is high
194-
desired_speed *= np.exp(-abs(ct_error)*0.4)
158+
# decay speed when crosstrack error is high
159+
desired_speed *= np.exp(-abs(e) * 0.4)
160+
195161
if desired_speed > self.speed_limit:
196-
desired_speed = self.speed_limit
197-
output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel)
162+
desired_speed = self.speed_limit
163+
164+
output_accel = self.pid_speed.advance(e=desired_speed - speed, t=t, feedforward_term=feedforward_accel)
165+
output_accel = np.clip(output_accel, -self.max_decel, self.max_accel)
166+
198167
if component is not None:
199-
component.debug('curr pt',(curr_x,curr_y))
200-
component.debug('curr param',self.current_path_parameter)
201-
component.debug('desired pt',(desired_x,desired_y))
202-
component.debug('desired yaw (rad)',desired_yaw)
203-
component.debug('crosstrack error',ct_error)
204-
component.debug('front wheel angle (rad)',f_delta)
205-
component.debug('desired speed (m/s)',desired_speed)
206-
component.debug('feedforward accel (m/s^2)',feedforward_accel)
207-
component.debug('output accel (m/s^2)',output_accel)
208-
print("Output accel: " + str(output_accel) + " m/s^2")
209-
210-
if output_accel > self.max_accel:
211-
output_accel = self.max_accel
212-
213-
if output_accel < -self.max_decel:
214-
output_accel = -self.max_decel
168+
component.debug('curr pt', (curr_x, curr_y))
169+
component.debug('closest path pt', path_point)
170+
component.debug('path tangent (rad)', path_yaw)
171+
component.debug('crosstrack error', e)
172+
component.debug('heading error (rad)', theta_e)
173+
component.debug('front wheel angle (rad)', f_delta)
174+
component.debug('desired speed (m/s)', desired_speed)
175+
component.debug('output accel (m/s^2)', output_accel)
215176

216177
self.t_last = t
217178
return (output_accel, f_delta)
218179

219180

220-
class PurePursuitTrajectoryTracker(Component):
221-
def __init__(self,vehicle_interface=None, **args):
222-
self.pure_pursuit = PurePursuit(**args)
181+
class StanleyTrajectoryTracker(Component):
182+
def __init__(self, vehicle_interface=None, **args):
183+
self.stanley = StanleyController(**args)
223184
self.vehicle_interface = vehicle_interface
224185

225186
def rate(self):
226187
return 50.0
227188

228189
def state_inputs(self):
229-
return ['vehicle','trajectory']
190+
return ['vehicle', 'trajectory']
230191

231192
def state_outputs(self):
232193
return []
233194

234-
def update(self, vehicle : VehicleState, trajectory: Trajectory):
235-
self.pure_pursuit.set_path(trajectory)
236-
accel,wheel_angle = self.pure_pursuit.compute(vehicle, self)
237-
#print("Desired wheel angle",wheel_angle)
238-
steering_angle = np.clip(front2steer(wheel_angle), self.pure_pursuit.steering_angle_range[0], self.pure_pursuit.steering_angle_range[1])
239-
#print("Desired steering angle",steering_angle)
240-
self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel,steering_angle, vehicle))
195+
def update(self, vehicle: VehicleState, trajectory: Trajectory):
196+
self.stanley.set_path(trajectory)
197+
accel, wheel_angle = self.stanley.compute(vehicle, self)
198+
steering_angle = np.clip(front2steer(wheel_angle),
199+
self.stanley.steering_angle_range[0],
200+
self.stanley.steering_angle_range[1])
201+
self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel, steering_angle, vehicle))
241202

242203
def healthy(self):
243-
return self.pure_pursuit.path is not None
204+
return self.stanley.path is not None

0 commit comments

Comments
 (0)