Spaces:
Paused
Paused
File size: 2,279 Bytes
7f3c2df |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 |
from sim.ilqr.lqr_solver import ILQRSolverParameters, ILQRWarmStartParameters, ILQRSolver
import numpy as np
solver_params = ILQRSolverParameters(
discretization_time=0.5,
state_cost_diagonal_entries=[1.0, 1.0, 10.0, 0.0, 0.0],
input_cost_diagonal_entries=[1.0, 10.0],
state_trust_region_entries=[1.0] * 5,
input_trust_region_entries=[1.0] * 2,
max_ilqr_iterations=100,
convergence_threshold=1e-6,
max_solve_time=0.05,
max_acceleration=3.0,
max_steering_angle=np.pi / 3.0,
max_steering_angle_rate=0.4,
min_velocity_linearization=0.01,
wheelbase=2.7
)
warm_start_params = ILQRWarmStartParameters(
k_velocity_error_feedback=0.5,
k_steering_angle_error_feedback=0.05,
lookahead_distance_lateral_error=15.0,
k_lateral_error=0.1,
jerk_penalty_warm_start_fit=1e-4,
curvature_rate_penalty_warm_start_fit=1e-2,
)
lqr = ILQRSolver(solver_params=solver_params, warm_start_params=warm_start_params)
def plan2control(plan_traj, init_state):
current_state = init_state
solutions = lqr.solve(current_state, plan_traj)
optimal_inputs = solutions[-1].input_trajectory
accel_cmd = optimal_inputs[0, 0]
steering_rate_cmd = optimal_inputs[0, 1]
return accel_cmd, steering_rate_cmd
if __name__ == '__main__':
# plan_traj = np.zeros((6,5))
# plan_traj[:, 0] = 1
# plan_traj[:, 1] = np.ones(6)
# plan_traj = np.cumsum(plan_traj, axis=0)
# print(plan_traj)
plan_traj = np.array([[-0.18724936, 2.29100776, 0., 0., 0., ],
[-0.29260731, 2.2971828 , 0., 0., 0. ],
[-0.46831554, 2.55596018, 0., 0., 0. ],
[-0.5859955 , 2.73183298, 0., 0., 0. ],
[-0.62684 , 2.84659386, 0., 0., 0. ],
[-0.67761713, 2.80647802, 0., 0., 0. ]])
plan_traj = plan_traj[:, [1,0,2,3,4]]
init_state = np.array([0.00000000e+00, 3.46944695e-17, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00])
print(plan_traj.shape, init_state.shape)
acc, steer = plan2control(plan_traj, init_state)
print(acc, steer)
|