Fixed bugs with collision handling
Fixed bugs with collision handling

moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX is necessary and there was a bug with usinf the wrong index.

WPI-ARC/trajectory_verifier