Successfully tuned PIDs for ros_control joint_trajectory_controller. Working on collision issue