compute virtual contact points for initial trajectory
only surfaces of normal.z > 0.5 can be contact plane
compute virtual contact points for initial trajectory
fix tunnelx90_2.dae rotation
update viz manager
update smoothness cost
modify yaml to use tunnelx90_2.dae
update cost weights