Added rviz config and relieved initial angle constraints
Added a parameter for initial angle cost