MPC does not rotate wheel
Hi, even though a valid trajectory is generated, the MPC is completely failing to turn the wheel inside the LGSVL sim (x64 foxy; it only commands acceleration and stopping), using ms3_sim launch. Here is a snippet of the ros2 topic echo control/control_diagnostic:
---
diag_header:
name: bicycle mpc controller qpoases
data_stamp:
sec: 1610250250
nanosec: 318266880
computation_start:
sec: 1610250259
nanosec: 91562995
runtime:
sec: 0
nanosec: 2242064
iterations: 0
new_trajectory: true
trajectory_source: ''
pose_source: ''
lateral_error_m: -27.149402618408203
longitudinal_error_m: -68.32011413574219
velocity_error_mps: -1.8953572511672974
acceleration_error_mps2: 0.0
yaw_error_rad: 9.196894645690918
yaw_rate_error_rps: 0.00042234035208821297
The returned command NEVER has any non-zero value for the state.front_wheel_angle_rad.
If you have seen this problem before, kindly share.